From 00104f0e58b8ccb2ffb52405b72a15e000bdfd62 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Mon, 17 Jun 2024 16:41:45 +0200 Subject: [PATCH] move dual arm task into branch --- motion_planners.orogen | 17 ------ tasks/DualArmPlannerTask.cpp | 115 ----------------------------------- tasks/DualArmPlannerTask.hpp | 108 -------------------------------- 3 files changed, 240 deletions(-) delete mode 100644 tasks/DualArmPlannerTask.cpp delete mode 100644 tasks/DualArmPlannerTask.hpp diff --git a/motion_planners.orogen b/motion_planners.orogen index c72853e..90fae1d 100644 --- a/motion_planners.orogen +++ b/motion_planners.orogen @@ -117,20 +117,3 @@ task_context "SingleArmPlannerTask", subclasses: "PlannerTask" do end -task_context "DualArmPlannerTask", subclasses: "PlannerTask" do - - # Configuration needed - needs_configuration - - # KLC Planner Config - property("klc_config", "/motion_planners/KLCConfig") - - input_port("target_joints_angle", "/base/commands/Joints"). - doc("Target joints angles") - - output_port("dual_arm_trajectory", "/base/JointsTrajectory"). - doc("The planned trajectory for the dual arm.") - - port_driven - -end diff --git a/tasks/DualArmPlannerTask.cpp b/tasks/DualArmPlannerTask.cpp deleted file mode 100644 index 58f4c0d..0000000 --- a/tasks/DualArmPlannerTask.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */ - -#include "DualArmPlannerTask.hpp" - -using namespace motion_planners; - -DualArmPlannerTask::DualArmPlannerTask(std::string const& name) - : DualArmPlannerTaskBase(name) -{ -} - -DualArmPlannerTask::~DualArmPlannerTask() -{ -} - - - -/// The following lines are template definitions for the various state machine -// hooks defined by Orocos::RTT. See DualArmPlannerTask.hpp for more detailed -// documentation about them. - -bool DualArmPlannerTask::configureHook() -{ - if (! DualArmPlannerTaskBase::configureHook()) - return false; - - klc_config_ = _klc_config.value(); - - dual_arm_planner_.reset(new motion_planners::DualArmMotionPlanners(config_, klc_config_)); - if(!dual_arm_planner_->initializeDualArmConfig(planner_status_)) - { - setPlannerStatus(planner_status_); - return false; - } - - return true; -} -bool DualArmPlannerTask::startHook() -{ - if (! DualArmPlannerTaskBase::startHook()) - return false; - return true; -} -void DualArmPlannerTask::updateHook() -{ - DualArmPlannerTaskBase::updateHook(); - - // This state is already is verified in the abstract class. - // TODO: Need to find a elegant solution, - if(state() == NO_JOINT_STATUS) - return; - - // adding or removing an object in the world. - if(_known_object.readNewest(known_object_) == RTT::NewData) - { - if(!dual_arm_planner_->handleCollisionObjectInWorld(known_object_)) - state(MODEL_OBJECT_ERROR); - } - - // adding or removing an object to the manipulator. - if(_grasp_object.readNewest(grasp_object_) == RTT::NewData) - { - if(!dual_arm_planner_->handleGraspObject(grasp_object_)) - state(MODEL_OBJECT_ERROR); - } - - // plan for target joint angles - if(_target_joints_angle.read(target_joints_angle_) == RTT::NewData) - { - updatePlanningscene(); - planner_status_.statuscode = motion_planners::PlannerStatus::INVALID; - state(GOAL_RECEIVED); - - if(dual_arm_planner_->assignDualArmPlanningRequest(joints_status_, target_joints_angle_, planner_status_)) - { - dual_arm_planner_->setStartAndGoal(); // this function will initialise the start and goal for the planner - state(PLANNING); - - if(dual_arm_planner_->solve(dual_arm_solution_, planner_status_, solving_time_)) - { - _dual_arm_trajectory.write(dual_arm_solution_); - } - - } - setPlannerStatus(planner_status_); - if(planner_status_.statuscode != PlannerStatus::PLANNING_REQUEST_SUCCESS) - _collided_links.write(dual_arm_planner_->getCollidedObjectsNames()); - } -} - -void DualArmPlannerTask::updatePlanningscene() -{ - if(convertEnvDataToOctomap()) - { - if(!initialised_planning_scene_) - { - dual_arm_planner_->assignOctomapPlanningScene(input_octree_); - } - - dual_arm_planner_->updateOctomap(input_octree_); - } -} - -void DualArmPlannerTask::errorHook() -{ - DualArmPlannerTaskBase::errorHook(); -} -void DualArmPlannerTask::stopHook() -{ - DualArmPlannerTaskBase::stopHook(); -} -void DualArmPlannerTask::cleanupHook() -{ - DualArmPlannerTaskBase::cleanupHook(); -} diff --git a/tasks/DualArmPlannerTask.hpp b/tasks/DualArmPlannerTask.hpp deleted file mode 100644 index a29416b..0000000 --- a/tasks/DualArmPlannerTask.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */ - -#ifndef MOTION_PLANNERS_DUALARMPLANNERTASK_TASK_HPP -#define MOTION_PLANNERS_DUALARMPLANNERTASK_TASK_HPP - -#include "motion_planners/DualArmPlannerTaskBase.hpp" -#include - -namespace motion_planners{ - - /*! \class DualArmPlannerTask - * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions. - * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. - * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. - * - * \details - * The name of a TaskContext is primarily defined via: - \verbatim - deployment 'deployment_name' - task('custom_task_name','motion_planners::DualArmPlannerTask') - end - \endverbatim - * It can be dynamically adapted when the deployment is called with a prefix argument. - */ - class DualArmPlannerTask : public DualArmPlannerTaskBase - { - friend class DualArmPlannerTaskBase; - protected: - std::unique_ptr dual_arm_planner_; - motion_planners::KLCConfig klc_config_; - base::commands::Joints target_joints_angle_; - base::JointsTrajectory dual_arm_solution_; - public: - /** TaskContext constructor for DualArmPlannerTask - * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices. - * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state. - */ - DualArmPlannerTask(std::string const& name = "motion_planners::DualArmPlannerTask"); - - /** Default deconstructor of DualArmPlannerTask - */ - ~DualArmPlannerTask(); - - /** This hook is called by Orocos when the state machine transitions - * from PreOperational to Stopped. If it returns false, then the - * component will stay in PreOperational. Otherwise, it goes into - * Stopped. - * - * It is meaningful only if the #needs_configuration has been specified - * in the task context definition with (for example): - \verbatim - task_context "TaskName" do - needs_configuration - ... - end - \endverbatim - */ - bool configureHook(); - - /** This hook is called by Orocos when the state machine transitions - * from Stopped to Running. If it returns false, then the component will - * stay in Stopped. Otherwise, it goes into Running and updateHook() - * will be called. - */ - bool startHook(); - - /** This hook is called by Orocos when the component is in the Running - * state, at each activity step. Here, the activity gives the "ticks" - * when the hook should be called. - * - * The error(), exception() and fatal() calls, when called in this hook, - * allow to get into the associated RunTimeError, Exception and - * FatalError states. - * - * In the first case, updateHook() is still called, and recover() allows - * you to go back into the Running state. In the second case, the - * errorHook() will be called instead of updateHook(). In Exception, the - * component is stopped and recover() needs to be called before starting - * it again. Finally, FatalError cannot be recovered. - */ - void updateHook(); - - /** This hook is called by Orocos when the component is in the - * RunTimeError state, at each activity step. See the discussion in - * updateHook() about triggering options. - * - * Call recover() to go back in the Runtime state. - */ - void errorHook(); - - /** This hook is called by Orocos when the state machine transitions - * from Running to Stopped after stop() has been called. - */ - void stopHook(); - - /** This hook is called by Orocos when the state machine transitions - * from Stopped to PreOperational, requiring the call to configureHook() - * before calling start() again. - */ - void cleanupHook(); - - private: - void updatePlanningscene( ); - }; -} - -#endif -