From d10d02eb319a8d6bfba11327a6b86e176cc6a277 Mon Sep 17 00:00:00 2001 From: gpollayil Date: Thu, 19 Sep 2019 09:32:38 +0200 Subject: [PATCH 01/21] Added exec depends for the metapackage --- panda_softhand/package.xml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/panda_softhand/package.xml b/panda_softhand/package.xml index 893125a..3d92324 100644 --- a/panda_softhand/package.xml +++ b/panda_softhand/package.xml @@ -1,5 +1,5 @@ - + panda_softhand 1.0.0 The panda_softhand package @@ -11,6 +11,11 @@ catkin + panda_softhand_description + panda_softhand_control + panda_softhand_safety + panda_softhand_moveit_config + From 31d639f3e8cea4beca84830ca19289b8bd5bd006 Mon Sep 17 00:00:00 2001 From: gpollayil Date: Thu, 19 Sep 2019 09:33:57 +0200 Subject: [PATCH 02/21] Added a gitignore file --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d039fd6 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +CATKIN_IGNORE \ No newline at end of file From a01a3ad3a64c0800710b48639d43d7fcdccb9e73 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Thu, 19 Sep 2019 17:04:51 +0200 Subject: [PATCH 03/21] Set correct robot ip --- panda_softhand_description/launch/launchPandaSoftHand.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda_softhand_description/launch/launchPandaSoftHand.launch b/panda_softhand_description/launch/launchPandaSoftHand.launch index dc1d6ad..79acd84 100644 --- a/panda_softhand_description/launch/launchPandaSoftHand.launch +++ b/panda_softhand_description/launch/launchPandaSoftHand.launch @@ -8,7 +8,7 @@ - + From c362ad7d74480cd9bfbeb232aba22168a2048a80 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Mon, 23 Sep 2019 13:09:17 +0200 Subject: [PATCH 04/21] Debug change --- panda_softhand_control/launch/launchSimulateObjectPose.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda_softhand_control/launch/launchSimulateObjectPose.launch b/panda_softhand_control/launch/launchSimulateObjectPose.launch index bfd05ae..42f7270 100644 --- a/panda_softhand_control/launch/launchSimulateObjectPose.launch +++ b/panda_softhand_control/launch/launchSimulateObjectPose.launch @@ -2,7 +2,7 @@ - From 5c0c7c93891b610c77599c18054cfb2b2ce5d64b Mon Sep 17 00:00:00 2001 From: mpollayil Date: Mon, 23 Sep 2019 13:10:15 +0200 Subject: [PATCH 05/21] Changed the topic of object pose and the lifted up the grasp pose --- panda_softhand_control/configs/task_sequences_params.yaml | 2 +- panda_softhand_control/src/TaskSequencer.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index aba96b2..eb8d95b 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -14,7 +14,7 @@ task_sequencer: # The joint configuration for home home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) - grasp_transform: [0.0, 0.0, 0.030, -3.1415, 0.0, 0.0] + grasp_transform: [0.0, 0.0, 0.150, -3.1415, 0.0, 0.0] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for handover diff --git a/panda_softhand_control/src/TaskSequencer.cpp b/panda_softhand_control/src/TaskSequencer.cpp index ac13d5f..ef119da 100644 --- a/panda_softhand_control/src/TaskSequencer.cpp +++ b/panda_softhand_control/src/TaskSequencer.cpp @@ -18,7 +18,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ } // Initializing the object subscriber and waiting (TODO: parse the topic name) - this->object_topic_name = "/object_pose"; + this->object_topic_name = "/irim_demo/chosen_object"; this->object_sub = this->nh.subscribe(this->object_topic_name, 1, &TaskSequencer::get_object_pose, this); ros::topic::waitForMessage(this->object_topic_name, ros::Duration(2.0)); @@ -251,6 +251,9 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_ tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff * pre_grasp_transform_aff, pre_grasp_pose); tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff, grasp_pose); + // Couting object pose for debugging + std::cout << "Object position is \n" << object_pose_aff.translation() << std::endl; + // 2) Going to pregrasp pose if(!this->panda_softhand_client.call_pose_service(pre_grasp_pose, false) || !this->franka_ok){ ROS_ERROR("Could not go to the specified pre grasp pose."); From 44384c3a6e200c84916da892cec5ffed1ce6bd1d Mon Sep 17 00:00:00 2001 From: mpollayil Date: Mon, 23 Sep 2019 14:59:53 +0200 Subject: [PATCH 06/21] Added the home, place and handover tasks services and needed variables. The object topic name is parsed now --- .../configs/task_sequences_params.yaml | 6 +- .../panda_softhand_control/TaskSequencer.h | 20 ++- panda_softhand_control/src/TaskSequencer.cpp | 121 ++++++++++++++++-- 3 files changed, 135 insertions(+), 12 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index eb8d95b..8c87949 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -10,13 +10,17 @@ task_sequencer: pos_controller: position_joint_trajectory_controller # Impedance controller imp_controller: cartesian_impedance_controller_softbots_stiff_matrix + # Name of the topic where the object pose is published + object_topic_name: /irim_demo/chosen_object # The joint configuration for home home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) - grasp_transform: [0.0, 0.0, 0.150, -3.1415, 0.0, 0.0] + grasp_transform: [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] + # The joint configuration for place + place_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006] # The joint configuration for handover handover_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006] # The threshold on tau_ext for handover hand opening diff --git a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h index d787130..ffa4020 100644 --- a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h +++ b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h @@ -56,6 +56,15 @@ class TaskSequencer { // Callback for simple grasp task service bool call_simple_grasp_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + // Callback for simple place task service + bool call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + + // Callback for simple home task service + bool call_simple_home_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + + // Callback for simple handover task service + bool call_simple_handover_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + // Callback for set object service bool call_set_object(panda_softhand_control::set_object::Request &req, panda_softhand_control::set_object::Response &res); @@ -84,14 +93,19 @@ class TaskSequencer { // The switch controller service name std::string switch_service_name = "/controller_manager/switch_controller"; - // Topic and service names - std::string object_topic_name; + // Service names std::string franka_state_topic_name = "/franka_state_controller/franka_states"; std::string grasp_task_service_name; + std::string place_task_service_name; + std::string home_task_service_name; + std::string handover_task_service_name; std::string set_object_service_name; // Service Servers ros::ServiceServer grasp_task_server; + ros::ServiceServer place_task_server; + ros::ServiceServer home_task_server; + ros::ServiceServer handover_task_server; ros::ServiceServer set_object_server; // The XmlRpc value for parsing complex params @@ -102,11 +116,13 @@ class TaskSequencer { std::string robot_joints_name; // Name of the robot joints (without the number of the joints) std::string pos_controller; // Name of position controller std::string imp_controller; // Name of impedance controller + std::string object_topic_name; // Name of the topic where the object pose is published std::vector home_joints; std::vector grasp_transform; geometry_msgs::Pose grasp_T; std::vector pre_grasp_transform; geometry_msgs::Pose pre_grasp_T; + std::vector place_joints; std::vector handover_joints; double handover_thresh; diff --git a/panda_softhand_control/src/TaskSequencer.cpp b/panda_softhand_control/src/TaskSequencer.cpp index ef119da..48379c0 100644 --- a/panda_softhand_control/src/TaskSequencer.cpp +++ b/panda_softhand_control/src/TaskSequencer.cpp @@ -17,8 +17,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ ROS_ERROR("The parsing of task parameters went wrong. Be careful, using default values..."); } - // Initializing the object subscriber and waiting (TODO: parse the topic name) - this->object_topic_name = "/irim_demo/chosen_object"; + // Initializing the object subscriber and waiting (the object topic name is parsed now) this->object_sub = this->nh.subscribe(this->object_topic_name, 1, &TaskSequencer::get_object_pose, this); ros::topic::waitForMessage(this->object_topic_name, ros::Duration(2.0)); @@ -35,10 +34,16 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ // Setting the task service names this->grasp_task_service_name = "grasp_task_service"; + this->place_task_service_name = "place_task_service"; + this->home_task_service_name = "home_task_service"; + this->handover_task_service_name = "handover_task_service"; this->set_object_service_name = "set_object_service"; // Advertising the services this->grasp_task_server = this->nh.advertiseService(this->grasp_task_service_name, &TaskSequencer::call_simple_grasp_task, this); + this->place_task_server = this->nh.advertiseService(this->place_task_service_name, &TaskSequencer::call_simple_place_task, this); + this->home_task_server = this->nh.advertiseService(this->home_task_service_name, &TaskSequencer::call_simple_home_task, this); + this->handover_task_server = this->nh.advertiseService(this->handover_task_service_name, &TaskSequencer::call_simple_handover_task, this); this->set_object_server = this->nh.advertiseService(this->set_object_service_name, &TaskSequencer::call_set_object, this); // Spinning once @@ -79,6 +84,12 @@ bool TaskSequencer::parse_task_params(){ success = false; } + if(!ros::param::get("/task_sequencer/object_topic_name", this->object_topic_name)){ + ROS_WARN("The param 'object_topic_name' not found in param server! Using default."); + this->object_topic_name = "cartesian_impedance_controller_softbots_stiff_matrix"; + success = false; + } + if(!ros::param::get("/task_sequencer/home_joints", this->home_joints)){ ROS_WARN("The param 'home_joints' not found in param server! Using default."); this->home_joints = {-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110}; @@ -111,6 +122,12 @@ bool TaskSequencer::parse_task_params(){ success = false; } + if(!ros::param::get("/task_sequencer/place_joints", this->place_joints)){ + ROS_WARN("The param 'place_joints' not found in param server! Using default."); + this->place_joints = {-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006}; + success = false; + } + if(!ros::param::get("/task_sequencer/handover_thresh", this->handover_thresh)){ ROS_WARN("The param 'handover_thresh' not found in param server! Using default."); this->handover_thresh = 4.5; @@ -286,15 +303,100 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_ return false; } - // 6) Going to handover joint config + // Now, everything finished well + res.success = true; + res.message = "The service call_simple_grasp_task was correctly performed!"; + return true; +} + +// Callback for simple place task service +bool TaskSequencer::call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){ + + // Checking the request for correctness + if(!req.data){ + ROS_WARN("Did you really want to call the simple place task service with data = false?"); + res.success = true; + res.message = "The service call_simple_place_task done correctly with false request!"; + return true; + } + + // 1) Going to place joint config + if(!this->panda_softhand_client.call_joint_service(this->place_joints) || !this->franka_ok){ + ROS_ERROR("Could not go to the specified place joint config."); + res.success = false; + res.message = "The service call_simple_place_task was NOT performed correctly!"; + return false; + } + + // 2) Opening hand + if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){ + ROS_ERROR("Could not open the hand."); + res.success = false; + res.message = "The service call_simple_place_task was NOT performed correctly!"; + return false; + } + + // Now, everything finished well + res.success = true; + res.message = "The service call_simple_place_task was correctly performed!"; + return true; + +} + +// Callback for simple home task service +bool TaskSequencer::call_simple_home_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){ + + // Checking the request for correctness + if(!req.data){ + ROS_WARN("Did you really want to call the simple home task service with data = false?"); + res.success = true; + res.message = "The service call_simple_home_task done correctly with false request!"; + return true; + } + + // 1) Going to home joint config + if(!this->panda_softhand_client.call_joint_service(this->home_joints) || !this->franka_ok){ + ROS_ERROR("Could not go to the specified home joint config."); + res.success = false; + res.message = "The service call_simple_home_task was NOT performed correctly!"; + return false; + } + + // 2) Opening hand + if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){ + ROS_ERROR("Could not open the hand."); + res.success = false; + res.message = "The service call_simple_home_task was NOT performed correctly!"; + return false; + } + + // Now, everything finished well + res.success = true; + res.message = "The service call_simple_home_task was correctly performed!"; + return true; + +} + +// Callback for simple handover task service +bool TaskSequencer::call_simple_handover_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){ + + // Checking the request for correctness + if(!req.data){ + ROS_WARN("Did you really want to call the simple handover task service with data = false?"); + res.success = true; + res.message = "The service call_simple_handover_task done correctly with false request!"; + return true; + } + + // 1) Going to handover joint config if(!this->panda_softhand_client.call_joint_service(this->handover_joints) || !this->franka_ok){ ROS_ERROR("Could not go to the specified handover joint config."); res.success = false; - res.message = "The service call_simple_grasp_task was NOT performed correctly!"; + res.message = "The service call_simple_handover_task was NOT performed correctly!"; return false; } - // 7) Waiting for threshold or for some time + // 2) Waiting for threshold or for some time sleep(1); // Sleeping for a second to avoid robot stopping peaks bool hand_open = false; ros::Time init_time = ros::Time::now(); ros::Time now_time; double base_tau_ext = this->tau_ext_norm; // Saving the present tau for later computation of variation @@ -314,21 +416,22 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_ } } - // 8) Opening hand + // 3) Opening hand if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){ ROS_ERROR("Could not open the hand."); res.success = false; - res.message = "The service call_simple_grasp_task was NOT performed correctly!"; + res.message = "The service call_simple_handover_task was NOT performed correctly!"; return false; } // Now, everything finished well res.success = true; - res.message = "The service call_simple_grasp_task was correctly performed!"; + res.message = "The service call_simple_handover_task was correctly performed!"; return true; + } -// Callback for handshake task service +// Callback for set object task service bool TaskSequencer::call_set_object(panda_softhand_control::set_object::Request &req, panda_softhand_control::set_object::Response &res){ // Checking if the parsed map contains the requested object From 3ce800165426bdc41e36c278e9bf1a070b2deb27 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Mon, 23 Sep 2019 15:11:31 +0200 Subject: [PATCH 07/21] Updated place joints and corrected the parsing --- panda_softhand_control/configs/task_sequences_params.yaml | 2 +- panda_softhand_control/src/TaskSequencer.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index 8c87949..16f4c3f 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -20,7 +20,7 @@ task_sequencer: # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for place - place_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006] + place_joints: [-0.136, 0.794, -0.115, -1.337, 0.250, 2.217, -0.479] # The joint configuration for handover handover_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006] # The threshold on tau_ext for handover hand opening diff --git a/panda_softhand_control/src/TaskSequencer.cpp b/panda_softhand_control/src/TaskSequencer.cpp index 48379c0..4365c43 100644 --- a/panda_softhand_control/src/TaskSequencer.cpp +++ b/panda_softhand_control/src/TaskSequencer.cpp @@ -86,7 +86,7 @@ bool TaskSequencer::parse_task_params(){ if(!ros::param::get("/task_sequencer/object_topic_name", this->object_topic_name)){ ROS_WARN("The param 'object_topic_name' not found in param server! Using default."); - this->object_topic_name = "cartesian_impedance_controller_softbots_stiff_matrix"; + this->object_topic_name = "/irim_demo/chosen_object"; success = false; } @@ -124,7 +124,7 @@ bool TaskSequencer::parse_task_params(){ if(!ros::param::get("/task_sequencer/place_joints", this->place_joints)){ ROS_WARN("The param 'place_joints' not found in param server! Using default."); - this->place_joints = {-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006}; + this->place_joints = {-0.136, 0.794, -0.115, -1.337, 0.250, 2.217, -0.479}; success = false; } From 699235906c46837d7a3c86cb20718ba79917b759 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Mon, 23 Sep 2019 17:26:34 +0200 Subject: [PATCH 08/21] Changed the objects grasp poses... This will be adjusted later --- .../configs/task_sequences_params.yaml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index 16f4c3f..34097ca 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -28,9 +28,6 @@ task_sequencer: # The personalized grasp pose maps for different objects poses_map: - "kettle" : [0.066, 0.012, 0.203, -2.973, 0.620, -0.032] - "teddy" : [0.046, -0.238, 0.106, 3.131, -0.052, -0.400] - "ball" : [-0.055, 0.113, 0.07, -3.104, 0.122, -0.241] - "two_cubes" : [-0.055, 0.113, 0.061, -3.104, 0.122, -0.241] - "bottle" : [0.068, 0.077, 0.019, -2.455, -0.249, 0.409] - "helmet" : [0.120, -0.039, 0.179, 3.007, 0.200, 0.247] \ No newline at end of file + "object1" : [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] + "object2" : [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] + "object3" : [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] \ No newline at end of file From 408ce169547809dac0efd94bc454018935f491f5 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 12:51:48 +0200 Subject: [PATCH 09/21] Added a wall for the camera in the planning scene --- .../worlds/panda_soma.world | 33 +++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world index 098296a..1a2de62 100755 --- a/panda_softhand_description/worlds/panda_soma.world +++ b/panda_softhand_description/worlds/panda_soma.world @@ -5,7 +5,7 @@ - + 0.6 0 -0.05 0 0 0 true @@ -24,7 +24,36 @@ - 1 1 0.05 + 1 1 0.05 + + + + + 0 + 1 + + + + + + 0.55 -0.55 0.2 0 0 0 + true + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + 0.5 0.02 1 + + + + + + + + 0.5 0.02 1 From fcee5f4a8e8f1516a97229f2e7befc5e56d6a43e Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 12:52:25 +0200 Subject: [PATCH 10/21] Added some other objects: still need to tune them though --- .../configs/task_sequences_params.yaml | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index 34097ca..c2a77c1 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -16,7 +16,8 @@ task_sequencer: # The joint configuration for home home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) - grasp_transform: [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] + # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default) + grasp_transform: [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for place @@ -28,6 +29,9 @@ task_sequencer: # The personalized grasp pose maps for different objects poses_map: - "object1" : [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] - "object2" : [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] - "object3" : [0.0, 0.0, 0.130, -3.1415, 0.0, 0.0] \ No newline at end of file + "object1" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] + "object2" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] + "object3" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] + "object4" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] + "object5" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] + "object6" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] \ No newline at end of file From 61038288a9836abc4816295066d2be46f4ab4884 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 13:14:11 +0200 Subject: [PATCH 11/21] Changed some incorrect find packages inside the sensor manager files for octomap in moveit --- panda_softhand_moveit_config/launch/sensor_manager.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml index 67aa2fd..3ae5854 100644 --- a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml +++ b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml @@ -3,7 +3,7 @@ - + @@ -13,6 +13,6 @@ - + From 085b0fcf75bfd128b64254008a1c69216d9195ab Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 15:18:09 +0200 Subject: [PATCH 12/21] Changes in grasp poses and place joint config --- .../configs/task_sequences_params.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index c2a77c1..8a6bbc3 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -17,11 +17,11 @@ task_sequencer: home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default) - grasp_transform: [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] + grasp_transform: [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for place - place_joints: [-0.136, 0.794, -0.115, -1.337, 0.250, 2.217, -0.479] + place_joints: [-0.068, 0.152, 0.193, -2.352, 0.0405, 2.519, 0.117] # The joint configuration for handover handover_joints: [-0.101, 0.161, 0.159, -1.651, 2.023, 2.419, -0.006] # The threshold on tau_ext for handover hand opening @@ -29,9 +29,9 @@ task_sequencer: # The personalized grasp pose maps for different objects poses_map: - "object1" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] - "object2" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] - "object3" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] - "object4" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] - "object5" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] - "object6" : [-0.04, -0.04, 0.145, -3.039, -0.185, -0.166] \ No newline at end of file + "object1" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] + "object2" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] + "object3" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] + "object4" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] + "object5" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] + "object6" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] \ No newline at end of file From 55547144978cb542d848980c7409bf41568eff24 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 17:36:00 +0200 Subject: [PATCH 13/21] Tried to disable octomap by changing the topic and frame names, but this seems not to work --- .../config/sensors_kinect_depthmap.yaml | 2 +- .../config/sensors_kinect_pointcloud.yaml | 2 +- panda_softhand_moveit_config/launch/sensor_manager.launch.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml b/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml index 9538fe0..064d797 100644 --- a/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml +++ b/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - image_topic: /camera/depth_registered/image_raw + image_topic: /trial/camera/depth_registered/image_raw queue_size: 5 near_clipping_plane_distance: 0.3 far_clipping_plane_distance: 5.0 diff --git a/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml b/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml index 92e7095..521c87f 100644 --- a/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml +++ b/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /camera/depth_registered/points + point_cloud_topic: /trial/camera/depth_registered/points max_range: 5.0 point_subsample: 1 padding_offset: 0.1 diff --git a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml index 3ae5854..b6a4788 100644 --- a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml +++ b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml @@ -7,7 +7,7 @@ - + From a80677c143c96c98385e4bcd3a4b41f82a36eff8 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 17:36:54 +0200 Subject: [PATCH 14/21] Shifted down the z coordinate of the table in the planning scene --- panda_softhand_description/worlds/panda_soma.world | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world index 1a2de62..5393c26 100755 --- a/panda_softhand_description/worlds/panda_soma.world +++ b/panda_softhand_description/worlds/panda_soma.world @@ -7,7 +7,7 @@ - 0.6 0 -0.05 0 0 0 + 0.6 0 -0.1 0 0 0 true 0 0 0 0 0 0 From 263cc67d8f030dc7324caf03500b4986275d201b Mon Sep 17 00:00:00 2001 From: mpollayil Date: Wed, 25 Sep 2019 17:37:10 +0200 Subject: [PATCH 15/21] Tuned the grasp poses --- .../configs/task_sequences_params.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index 8a6bbc3..2888105 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -17,7 +17,7 @@ task_sequencer: home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default) - grasp_transform: [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] + grasp_transform: [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for place @@ -29,9 +29,9 @@ task_sequencer: # The personalized grasp pose maps for different objects poses_map: - "object1" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] - "object2" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] - "object3" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] - "object4" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] - "object5" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] - "object6" : [-0.04, -0.04, 0.05, -3.039, -0.185, -0.166] \ No newline at end of file + "object1" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] + "object2" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] + "object3" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] + "object4" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] + "object5" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] + "object6" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] \ No newline at end of file From 42ce0735faffce1b1ef28b3f3eaa10eb5b4ecf0d Mon Sep 17 00:00:00 2001 From: mpollayil Date: Thu, 26 Sep 2019 10:16:33 +0200 Subject: [PATCH 16/21] Changed the error in find package and disabled octomap --- panda_softhand_moveit_config/CHANGELOG.rst | 4 ++-- .../config/panda_arm.srdf.xacro | 2 +- .../config/panda_arm_hand.srdf.xacro | 4 ++-- .../config/sensors_kinect_depthmap.yaml | 2 +- .../config/sensors_kinect_pointcloud.yaml | 2 +- .../launch/chomp_planning_pipeline.launch.xml | 2 +- .../launch/default_warehouse_db.launch | 4 ++-- panda_softhand_moveit_config/launch/demo.launch | 10 +++++----- panda_softhand_moveit_config/launch/demo_chomp.launch | 10 +++++----- .../launch/fake_moveit_controller_manager.launch.xml | 2 +- panda_softhand_moveit_config/launch/move_group.launch | 10 +++++----- panda_softhand_moveit_config/launch/moveit_rviz.launch | 6 +++--- .../launch/ompl_planning_pipeline.launch.xml | 2 +- .../launch/panda_control_moveit_rviz.launch | 4 ++-- .../panda_gripper_moveit_controller_manager.launch.xml | 2 +- .../launch/panda_moveit.launch | 2 +- .../launch/panda_moveit_controller_manager.launch.xml | 2 +- .../launch/planning_context.launch | 8 ++++---- .../launch/planning_pipeline.launch.xml | 2 +- .../launch/run_benchmark_ompl.launch | 8 ++++---- .../launch/sensor_manager.launch.xml | 3 ++- .../launch/setup_assistant.launch | 2 +- .../launch/stomp_planning_pipeline.launch.xml | 2 +- .../launch/trajectory_execution.launch.xml | 2 +- panda_softhand_moveit_config/launch/warehouse.launch | 2 +- panda_softhand_moveit_config/package.xml | 2 +- 26 files changed, 51 insertions(+), 50 deletions(-) diff --git a/panda_softhand_moveit_config/CHANGELOG.rst b/panda_softhand_moveit_config/CHANGELOG.rst index 9b32223..14f8757 100644 --- a/panda_softhand_moveit_config/CHANGELOG.rst +++ b/panda_softhand_moveit_config/CHANGELOG.rst @@ -1,9 +1,9 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package panda_moveit_config +Changelog for package panda_softhand_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.0 (2018-11-09) ------------------ -* Initial release of panda_moveit_config into Melodic, including OMPL, CHOMP and STOMP configs +* Initial release of panda_softhand_moveit_config into Melodic, including OMPL, CHOMP and STOMP configs We moved/merged this repo from https://github.com/frankaemika/franka_ros. * Contributors: Dave Coleman, Florian Walch, Mike Lautman, Raghavender Sahdev diff --git a/panda_softhand_moveit_config/config/panda_arm.srdf.xacro b/panda_softhand_moveit_config/config/panda_arm.srdf.xacro index 31575d6..e267c8e 100644 --- a/panda_softhand_moveit_config/config/panda_arm.srdf.xacro +++ b/panda_softhand_moveit_config/config/panda_arm.srdf.xacro @@ -4,6 +4,6 @@ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + diff --git a/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro b/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro index 39f7db9..4c738b3 100644 --- a/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro +++ b/panda_softhand_moveit_config/config/panda_arm_hand.srdf.xacro @@ -4,8 +4,8 @@ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - - + + diff --git a/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml b/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml index 064d797..9538fe0 100644 --- a/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml +++ b/panda_softhand_moveit_config/config/sensors_kinect_depthmap.yaml @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - image_topic: /trial/camera/depth_registered/image_raw + image_topic: /camera/depth_registered/image_raw queue_size: 5 near_clipping_plane_distance: 0.3 far_clipping_plane_distance: 5.0 diff --git a/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml b/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml index 521c87f..e870208 100644 --- a/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml +++ b/panda_softhand_moveit_config/config/sensors_kinect_pointcloud.yaml @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /trial/camera/depth_registered/points + point_cloud_topic: /trial_camera/depth_registered/points # Added a trial_ in front to disable octomap max_range: 5.0 point_subsample: 1 padding_offset: 0.1 diff --git a/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml index ffbc72e..7e5d445 100644 --- a/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml +++ b/panda_softhand_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -10,6 +10,6 @@ - + diff --git a/panda_softhand_moveit_config/launch/default_warehouse_db.launch b/panda_softhand_moveit_config/launch/default_warehouse_db.launch index 6901960..2e60c9d 100644 --- a/panda_softhand_moveit_config/launch/default_warehouse_db.launch +++ b/panda_softhand_moveit_config/launch/default_warehouse_db.launch @@ -2,10 +2,10 @@ - + - + diff --git a/panda_softhand_moveit_config/launch/demo.launch b/panda_softhand_moveit_config/launch/demo.launch index a8a9a86..bd87314 100644 --- a/panda_softhand_moveit_config/launch/demo.launch +++ b/panda_softhand_moveit_config/launch/demo.launch @@ -3,7 +3,7 @@ - + @@ -20,7 +20,7 @@ - + @@ -38,7 +38,7 @@ - + @@ -46,13 +46,13 @@ - + - + diff --git a/panda_softhand_moveit_config/launch/demo_chomp.launch b/panda_softhand_moveit_config/launch/demo_chomp.launch index 1ae28f9..8d697e5 100644 --- a/panda_softhand_moveit_config/launch/demo_chomp.launch +++ b/panda_softhand_moveit_config/launch/demo_chomp.launch @@ -3,7 +3,7 @@ - + @@ -20,7 +20,7 @@ - + @@ -38,7 +38,7 @@ - + @@ -47,13 +47,13 @@ - + - + diff --git a/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml index c95563d..71d07a0 100644 --- a/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ b/panda_softhand_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -4,6 +4,6 @@ - + diff --git a/panda_softhand_moveit_config/launch/move_group.launch b/panda_softhand_moveit_config/launch/move_group.launch index ccc7150..528cec0 100644 --- a/panda_softhand_moveit_config/launch/move_group.launch +++ b/panda_softhand_moveit_config/launch/move_group.launch @@ -2,7 +2,7 @@ - + @@ -12,7 +12,7 @@ + value="gdb -x $(find panda_softhand_moveit_config)/launch/gdb_settings.gdb --ex run --args" /> @@ -27,12 +27,12 @@ - + - + @@ -40,7 +40,7 @@ - + diff --git a/panda_softhand_moveit_config/launch/moveit_rviz.launch b/panda_softhand_moveit_config/launch/moveit_rviz.launch index 016e5dc..82b2e8a 100644 --- a/panda_softhand_moveit_config/launch/moveit_rviz.launch +++ b/panda_softhand_moveit_config/launch/moveit_rviz.launch @@ -5,12 +5,12 @@ - - + + - + diff --git a/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml index f6794d5..0ee39e4 100644 --- a/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/panda_softhand_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -17,6 +17,6 @@ - + diff --git a/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch b/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch index 748f260..ec02a62 100644 --- a/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch +++ b/panda_softhand_moveit_config/launch/panda_control_moveit_rviz.launch @@ -9,9 +9,9 @@ - + - + diff --git a/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml b/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml index 27ffcea..db59fa1 100644 --- a/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml +++ b/panda_softhand_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml @@ -3,5 +3,5 @@ - + diff --git a/panda_softhand_moveit_config/launch/panda_moveit.launch b/panda_softhand_moveit_config/launch/panda_moveit.launch index 8559413..550a445 100644 --- a/panda_softhand_moveit_config/launch/panda_moveit.launch +++ b/panda_softhand_moveit_config/launch/panda_moveit.launch @@ -4,7 +4,7 @@ - + diff --git a/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml b/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml index ea4c0ad..a09b324 100644 --- a/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml +++ b/panda_softhand_moveit_config/launch/panda_moveit_controller_manager.launch.xml @@ -3,5 +3,5 @@ - + diff --git a/panda_softhand_moveit_config/launch/planning_context.launch b/panda_softhand_moveit_config/launch/planning_context.launch index e961339..5b0af6b 100644 --- a/panda_softhand_moveit_config/launch/planning_context.launch +++ b/panda_softhand_moveit_config/launch/planning_context.launch @@ -12,17 +12,17 @@ - - + + - + - + diff --git a/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml index ac6838d..ce686a5 100644 --- a/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml +++ b/panda_softhand_moveit_config/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - + diff --git a/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch b/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch index 6bf8ba2..0b0a4ff 100644 --- a/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch +++ b/panda_softhand_moveit_config/launch/run_benchmark_ompl.launch @@ -4,19 +4,19 @@ - + - + - - + + diff --git a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml index b6a4788..38922ad 100644 --- a/panda_softhand_moveit_config/launch/sensor_manager.launch.xml +++ b/panda_softhand_moveit_config/launch/sensor_manager.launch.xml @@ -7,7 +7,8 @@ - + + diff --git a/panda_softhand_moveit_config/launch/setup_assistant.launch b/panda_softhand_moveit_config/launch/setup_assistant.launch index 95d88d7..274ba52 100644 --- a/panda_softhand_moveit_config/launch/setup_assistant.launch +++ b/panda_softhand_moveit_config/launch/setup_assistant.launch @@ -8,7 +8,7 @@ diff --git a/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml b/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml index 7e1c5b8..d390a01 100644 --- a/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml +++ b/panda_softhand_moveit_config/launch/stomp_planning_pipeline.launch.xml @@ -14,6 +14,6 @@ - + diff --git a/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml b/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml index b6c03c9..1bffbea 100644 --- a/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml +++ b/panda_softhand_moveit_config/launch/trajectory_execution.launch.xml @@ -15,6 +15,6 @@ - + diff --git a/panda_softhand_moveit_config/launch/warehouse.launch b/panda_softhand_moveit_config/launch/warehouse.launch index a4a1374..8bf3f18 100644 --- a/panda_softhand_moveit_config/launch/warehouse.launch +++ b/panda_softhand_moveit_config/launch/warehouse.launch @@ -4,7 +4,7 @@ - + diff --git a/panda_softhand_moveit_config/package.xml b/panda_softhand_moveit_config/package.xml index be8ecca..dfb8466 100644 --- a/panda_softhand_moveit_config/package.xml +++ b/panda_softhand_moveit_config/package.xml @@ -2,7 +2,7 @@ panda_softhand_moveit_config 1.0.0 - A package, based on panda_moveit_config, with all the configuration and launch files for using the Panda and Pisa/IIT SoftHand with MoveIt! + A package, based on panda_softhand_moveit_config, with all the configuration and launch files for using the Panda and Pisa/IIT SoftHand with MoveIt! George Jose Pollayil Mathew Jose Pollayil From de3d2e5921ad39d0ed2aaea04fb79af4d02add52 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Thu, 26 Sep 2019 15:05:52 +0200 Subject: [PATCH 17/21] Adjusted grasp poses and added boxes in planning scene --- .../configs/task_sequences_params.yaml | 14 +-- .../worlds/panda_soma.world | 93 ++++++++++++++++++- 2 files changed, 97 insertions(+), 10 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index 2888105..36d664c 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -17,7 +17,7 @@ task_sequencer: home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default) - grasp_transform: [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] + grasp_transform: [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for place @@ -29,9 +29,9 @@ task_sequencer: # The personalized grasp pose maps for different objects poses_map: - "object1" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] - "object2" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] - "object3" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] - "object4" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] - "object5" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] - "object6" : [-0.06, -0.04, 0.08, -3.039, -0.185, -0.166] \ No newline at end of file + "object1" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] + "object2" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] + "object3" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] + "object4" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] + "object5" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] + "object6" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] \ No newline at end of file diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world index 5393c26..78aaf3f 100755 --- a/panda_softhand_description/worlds/panda_soma.world +++ b/panda_softhand_description/worlds/panda_soma.world @@ -4,10 +4,39 @@ + + + + -0.08 0 -0.03 0 0 0 + true + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + 0.30 1 0.04 + + + + + + + + 0.30 1 0.04 + + + + + 0 + 1 + + - 0.6 0 -0.1 0 0 0 + 0.6 0 -0.06 0 0 0 true 0 0 0 0 0 0 @@ -16,7 +45,7 @@ 0 0 0 0 0 0 - 1 1 0.05 + 1.2 1 0.03 @@ -24,7 +53,7 @@ - 1 1 0.05 + 1.2 1 0.03 @@ -63,6 +92,64 @@ + + + 0 0.55 0.2 0 0 0 + true + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + 0.5 0.02 0.5 + + + + + + + + 0.5 0.02 0.5 + + + + + 0 + 1 + + + + + + 0.65 0.13 0.01 0 0 0 + true + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + 0.35 0.27 0.08 + + + + + + + + 0.35 0.27 0.08 + + + + + 0 + 1 + + + From c47fe4bef3ce037895a9a2bef2e72987a006286a Mon Sep 17 00:00:00 2001 From: mpollayil Date: Thu, 26 Sep 2019 17:32:37 +0200 Subject: [PATCH 18/21] Added a new grasp service which takes as request a pose and does not listen to a tf --- panda_softhand_control/CMakeLists.txt | 1 + .../panda_softhand_control/TaskSequencer.h | 6 ++ panda_softhand_control/src/TaskSequencer.cpp | 73 +++++++++++++++++++ panda_softhand_control/srv/complex_grasp.srv | 5 ++ 4 files changed, 85 insertions(+) create mode 100644 panda_softhand_control/srv/complex_grasp.srv diff --git a/panda_softhand_control/CMakeLists.txt b/panda_softhand_control/CMakeLists.txt index 9cd9880..6de362e 100644 --- a/panda_softhand_control/CMakeLists.txt +++ b/panda_softhand_control/CMakeLists.txt @@ -35,6 +35,7 @@ add_service_files(FILES pose_control.srv joint_control.srv set_object.srv + complex_grasp.srv ) generate_messages(DEPENDENCIES diff --git a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h index ffa4020..ebd4bbf 100644 --- a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h +++ b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h @@ -12,6 +12,7 @@ Email: gpollayil@gmail.com, mathewjosepollayil@gmail.com */ #include "std_msgs/Bool.h" #include "std_srvs/SetBool.h" #include "panda_softhand_control/set_object.h" +#include "panda_softhand_control/complex_grasp.h" #include "geometry_msgs/Pose.h" #include #include @@ -56,6 +57,9 @@ class TaskSequencer { // Callback for simple grasp task service bool call_simple_grasp_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + // Callback for complex grasp task service (goes to specified pose) + bool call_complex_grasp_task(panda_softhand_control::complex_grasp::Request &req, panda_softhand_control::complex_grasp::Response &res); + // Callback for simple place task service bool call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); @@ -96,6 +100,7 @@ class TaskSequencer { // Service names std::string franka_state_topic_name = "/franka_state_controller/franka_states"; std::string grasp_task_service_name; + std::string complex_grasp_task_service_name; std::string place_task_service_name; std::string home_task_service_name; std::string handover_task_service_name; @@ -103,6 +108,7 @@ class TaskSequencer { // Service Servers ros::ServiceServer grasp_task_server; + ros::ServiceServer complex_grasp_task_server; ros::ServiceServer place_task_server; ros::ServiceServer home_task_server; ros::ServiceServer handover_task_server; diff --git a/panda_softhand_control/src/TaskSequencer.cpp b/panda_softhand_control/src/TaskSequencer.cpp index 4365c43..7d10c98 100644 --- a/panda_softhand_control/src/TaskSequencer.cpp +++ b/panda_softhand_control/src/TaskSequencer.cpp @@ -34,6 +34,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ // Setting the task service names this->grasp_task_service_name = "grasp_task_service"; + this->complex_grasp_task_service_name = "complex_grasp_task_service"; this->place_task_service_name = "place_task_service"; this->home_task_service_name = "home_task_service"; this->handover_task_service_name = "handover_task_service"; @@ -41,6 +42,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ // Advertising the services this->grasp_task_server = this->nh.advertiseService(this->grasp_task_service_name, &TaskSequencer::call_simple_grasp_task, this); + this->complex_grasp_task_server = this->nh.advertiseService(this->complex_grasp_task_service_name, &TaskSequencer::call_complex_grasp_task, this); this->place_task_server = this->nh.advertiseService(this->place_task_service_name, &TaskSequencer::call_simple_place_task, this); this->home_task_server = this->nh.advertiseService(this->home_task_service_name, &TaskSequencer::call_simple_home_task, this); this->handover_task_server = this->nh.advertiseService(this->handover_task_service_name, &TaskSequencer::call_simple_handover_task, this); @@ -309,6 +311,77 @@ bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_ return true; } +// Callback for complex grasp task service (goes to specified pose) +bool TaskSequencer::call_complex_grasp_task(panda_softhand_control::complex_grasp::Request &req, panda_softhand_control::complex_grasp::Response &res){ + + // Checking the request for correctness + if(!req.data){ + ROS_WARN("Did you really want to call the complex grasp task service with data = false?"); + res.success = true; + res.message = "The service call_complex_grasp_task done correctly with false request!"; + return true; + } + + // 1) Going to home configuration + if(!this->panda_softhand_client.call_joint_service(this->home_joints) || !this->franka_ok){ + ROS_ERROR("Could not go to the specified home joint configuration."); + res.success = false; + res.message = "The service call_complex_grasp_task was NOT performed correctly!"; + return false; + } + + // Computing the grasp and pregrasp pose and converting to geometry_msgs Pose + Eigen::Affine3d object_pose_aff; tf::poseMsgToEigen(req.object_pose, object_pose_aff); + Eigen::Affine3d grasp_transform_aff; tf::poseMsgToEigen(this->grasp_T, grasp_transform_aff); + Eigen::Affine3d pre_grasp_transform_aff; tf::poseMsgToEigen(this->pre_grasp_T, pre_grasp_transform_aff); + + geometry_msgs::Pose pre_grasp_pose; geometry_msgs::Pose grasp_pose; + tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff * pre_grasp_transform_aff, pre_grasp_pose); + tf::poseEigenToMsg(object_pose_aff * grasp_transform_aff, grasp_pose); + + // Couting object pose for debugging + std::cout << "Object position is \n" << object_pose_aff.translation() << std::endl; + + // 2) Going to pregrasp pose + if(!this->panda_softhand_client.call_pose_service(pre_grasp_pose, false) || !this->franka_ok){ + ROS_ERROR("Could not go to the specified pre grasp pose."); + res.success = false; + res.message = "The service call_complex_grasp_task was NOT performed correctly!"; + return false; + } + + // 3) Going to grasp pose + if(!this->panda_softhand_client.call_slerp_service(grasp_pose, false) || !this->franka_ok){ + ROS_ERROR("Could not go to the specified grasp pose."); + res.success = false; + res.message = "The service call_complex_grasp_task was NOT performed correctly!"; + return false; + } + + // 4) Performing complex grasp + if(!this->panda_softhand_client.call_hand_service(1.0, 2.0) || !this->franka_ok){ + ROS_ERROR("Could not perform the complex grasp."); + res.success = false; + res.message = "The service call_complex_grasp_task was NOT performed correctly!"; + return false; + } + + // 4) Lifting the grasped? object + if(!this->panda_softhand_client.call_joint_service(this->home_joints) || !this->franka_ok){ + ROS_ERROR("Could not lift to the specified pose."); + res.success = false; + res.message = "The service call_complex_grasp_task was NOT performed correctly!"; + return false; + } + + // Now, everything finished well + res.success = true; + res.message = "The service call_complex_grasp_task was correctly performed!"; + return true; + +} + + // Callback for simple place task service bool TaskSequencer::call_simple_place_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){ diff --git a/panda_softhand_control/srv/complex_grasp.srv b/panda_softhand_control/srv/complex_grasp.srv new file mode 100644 index 0000000..1e73f72 --- /dev/null +++ b/panda_softhand_control/srv/complex_grasp.srv @@ -0,0 +1,5 @@ +bool data +geometry_msgs/Pose object_pose +--- +bool success +string message \ No newline at end of file From 17e4ac92ae2c69bf2d0d2b85f02428abae10fd3b Mon Sep 17 00:00:00 2001 From: mpollayil Date: Thu, 26 Sep 2019 18:34:00 +0200 Subject: [PATCH 19/21] Finished adjusting the world for planning scene --- .../worlds/panda_soma.world | 45 +++++++++++++++---- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world index 78aaf3f..2179f07 100755 --- a/panda_softhand_description/worlds/panda_soma.world +++ b/panda_softhand_description/worlds/panda_soma.world @@ -63,9 +63,38 @@ + + + -0.50 0 0.5 0 0 0 + true + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + 0.05 1 1 + + + + + + + + 0.05 1 1 + + + + + 0 + 1 + + + - 0.55 -0.55 0.2 0 0 0 + 0.50 -0.55 0.2 0 0 0 true 0 0 0 0 0 0 @@ -74,7 +103,7 @@ 0 0 0 0 0 0 - 0.5 0.02 1 + 1 0.02 1 @@ -82,7 +111,7 @@ - 0.5 0.02 1 + 1 0.02 1 @@ -94,7 +123,7 @@ - 0 0.55 0.2 0 0 0 + 0.50 0.55 0.2 0 0 0 true 0 0 0 0 0 0 @@ -103,7 +132,7 @@ 0 0 0 0 0 0 - 0.5 0.02 0.5 + 1 0.02 1 @@ -111,7 +140,7 @@ - 0.5 0.02 0.5 + 1 0.02 1 @@ -132,7 +161,7 @@ 0 0 0 0 0 0 - 0.35 0.27 0.08 + 0.37 0.29 0.1 @@ -140,7 +169,7 @@ - 0.35 0.27 0.08 + 0.37 0.29 0.1 From 148382d3f1a63ebe39209ecf6e044a3df5212ad9 Mon Sep 17 00:00:00 2001 From: mpollayil Date: Fri, 4 Oct 2019 12:53:29 +0200 Subject: [PATCH 20/21] Minor param changes done during demo --- .../configs/task_sequences_params.yaml | 14 +++++++------- .../worlds/panda_soma.world | 16 ++++++++-------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/panda_softhand_control/configs/task_sequences_params.yaml b/panda_softhand_control/configs/task_sequences_params.yaml index 36d664c..91acb5a 100644 --- a/panda_softhand_control/configs/task_sequences_params.yaml +++ b/panda_softhand_control/configs/task_sequences_params.yaml @@ -17,7 +17,7 @@ task_sequencer: home_joints: [-0.035, -0.109, -0.048, -1.888, 0.075, 1.797, -0.110] # Transform from object frame to grasp pose frame (xyzrpy) (DEFAULT) # grasp_transform: [-0.04, -0.04, 0.145, -3.1415, 0.0, 0.0] (default) - grasp_transform: [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] + grasp_transform: [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] # Transform from grasp pose frame to pre-grasp pose frame (xyzrpy) pre_grasp_transform: [0.0, 0.0, -0.1, 0.0, 0.0, 0.0] # The joint configuration for place @@ -29,9 +29,9 @@ task_sequencer: # The personalized grasp pose maps for different objects poses_map: - "object1" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] - "object2" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] - "object3" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] - "object4" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] - "object5" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] - "object6" : [-0.06, -0.04, 0.025, -3.039, -0.185, -0.166] \ No newline at end of file + "object1" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] + "object2" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] + "object3" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] + "object4" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] + "object5" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] + "object6" : [-0.08, -0.05, 0.025, -3.039, -0.185, -0.166] \ No newline at end of file diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world index 2179f07..ca479f4 100755 --- a/panda_softhand_description/worlds/panda_soma.world +++ b/panda_softhand_description/worlds/panda_soma.world @@ -36,7 +36,7 @@ - 0.6 0 -0.06 0 0 0 + 0.6 0 -0.05 0 0 0 true 0 0 0 0 0 0 @@ -65,7 +65,7 @@ - -0.50 0 0.5 0 0 0 + -0.30 0 0.5 0 0 0 true 0 0 0 0 0 0 @@ -94,7 +94,7 @@ - 0.50 -0.55 0.2 0 0 0 + 0.20 -0.49 0.2 0 0 0 true 0 0 0 0 0 0 @@ -103,7 +103,7 @@ 0 0 0 0 0 0 - 1 0.02 1 + 1.5 0.02 1 @@ -111,7 +111,7 @@ - 1 0.02 1 + 1.5 0.02 1 @@ -123,7 +123,7 @@ - 0.50 0.55 0.2 0 0 0 + 0.20 0.30 0.2 0 0 0 true 0 0 0 0 0 0 @@ -132,7 +132,7 @@ 0 0 0 0 0 0 - 1 0.02 1 + 1.5 0.02 1 @@ -140,7 +140,7 @@ - 1 0.02 1 + 1.5 0.02 1 From 3794835577ef05948439bdb38537f1f7ac5cc448 Mon Sep 17 00:00:00 2001 From: George Jose Pollayil Date: Thu, 10 Mar 2022 13:43:02 +0100 Subject: [PATCH 21/21] Changed world and corrected error with header --- .../panda_softhand_control/TaskSequencer.h | 4 +- panda_softhand_control/src/TaskSequencer.cpp | 30 ++++---- .../configs/panda_soma.rviz | 20 +++-- .../launch/launchPandaSoftHand.launch | 14 ++-- .../worlds/panda_soma.world | 76 +++++++++---------- .../config/panda_arm.xacro | 25 ++++++ 6 files changed, 100 insertions(+), 69 deletions(-) diff --git a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h index ebd4bbf..a9b7e80 100644 --- a/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h +++ b/panda_softhand_control/include/panda_softhand_control/TaskSequencer.h @@ -16,7 +16,7 @@ Email: gpollayil@gmail.com, mathewjosepollayil@gmail.com */ #include "geometry_msgs/Pose.h" #include #include -#include +#include // Parsing includes #include @@ -134,4 +134,4 @@ class TaskSequencer { std::map> poses_map; // The map containing the notable poses -}; \ No newline at end of file +}; diff --git a/panda_softhand_control/src/TaskSequencer.cpp b/panda_softhand_control/src/TaskSequencer.cpp index 7d10c98..9a987ef 100644 --- a/panda_softhand_control/src/TaskSequencer.cpp +++ b/panda_softhand_control/src/TaskSequencer.cpp @@ -8,7 +8,7 @@ Email: gpollayil@gmail.com, mathewjosepollayil@gmail.com */ #include "panda_softhand_control/TaskSequencer.h" TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ - + // Initializing Node Handle this->nh = nh_; @@ -26,7 +26,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ ros::topic::waitForMessage("/" + this->robot_name + this->franka_state_topic_name, ros::Duration(2.0)); // Initializing the tau_ext norm and franka recovery publishers - this->pub_franka_recovery = this->nh.advertise("/" + this->robot_name + "/franka_control/error_recovery/goal", 1); + this->pub_franka_recovery = this->nh.advertise("/" + this->robot_name + "/franka_control/error_recovery/goal", 1); this->pub_tau_ext_norm = this->nh.advertise("tau_ext_norm", 1); // Initializing Panda SoftHand Client (TODO: Return error if initialize returns false) @@ -54,7 +54,7 @@ TaskSequencer::TaskSequencer(ros::NodeHandle& nh_){ } TaskSequencer::~TaskSequencer(){ - + // Nothing to do here yet } @@ -153,8 +153,8 @@ bool TaskSequencer::parse_task_params(){ std::cout << it.first << " : [ "; for(auto vec_it : it.second){ std::cout << vec_it << " "; - } - std::cout << "]" << std::endl; + } + std::cout << "]" << std::endl; } } @@ -163,7 +163,7 @@ bool TaskSequencer::parse_task_params(){ // Convert xyzrpy vector to geometry_msgs Pose geometry_msgs::Pose TaskSequencer::convert_vector_to_pose(std::vector input_vec){ - + // Creating temporary variables geometry_msgs::Pose output_pose; Eigen::Affine3d output_affine; @@ -174,8 +174,8 @@ geometry_msgs::Pose TaskSequencer::convert_vector_to_pose(std::vector in Eigen::Matrix3d rotation = Eigen::Matrix3d(Eigen::AngleAxisd(input_vec[5], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(input_vec[4], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(input_vec[3], Eigen::Vector3d::UnitX())); - output_affine.linear() = rotation; - + output_affine.linear() = rotation; + // Converting to geometry_msgs and returning tf::poseEigenToMsg(output_affine, output_pose); return output_pose; @@ -239,12 +239,12 @@ void TaskSequencer::get_franka_state(const franka_msgs::FrankaState::ConstPtr &m // Publishing norm std_msgs::Float64 norm_msg; norm_msg.data = this->tau_ext_norm; this->pub_tau_ext_norm.publish(norm_msg); - + } // Callback for simple grasp task service bool TaskSequencer::call_simple_grasp_task(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res){ - + // Checking the request for correctness if(!req.data){ ROS_WARN("Did you really want to call the simple grasp task service with data = false?"); @@ -401,7 +401,7 @@ bool TaskSequencer::call_simple_place_task(std_srvs::SetBool::Request &req, std_ return false; } - // 2) Opening hand + // 2) Opening hand if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){ ROS_ERROR("Could not open the hand."); res.success = false; @@ -435,7 +435,7 @@ bool TaskSequencer::call_simple_home_task(std_srvs::SetBool::Request &req, std_s return false; } - // 2) Opening hand + // 2) Opening hand if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){ ROS_ERROR("Could not open the hand."); res.success = false; @@ -484,12 +484,12 @@ bool TaskSequencer::call_simple_handover_task(std_srvs::SetBool::Request &req, s if((now_time - init_time) > ros::Duration(10, 0)){ hand_open = true; if(DEBUG) ROS_WARN_STREAM("Opening condition reached!" << " TIMEOUT!"); - if(DEBUG) ROS_WARN_STREAM("The initial time was " << init_time << ", now it is " << now_time + if(DEBUG) ROS_WARN_STREAM("The initial time was " << init_time << ", now it is " << now_time << ", the difference is " << (now_time - init_time) << " and the timeout thresh is " << ros::Duration(10, 0)); } } - // 3) Opening hand + // 3) Opening hand if(!this->panda_softhand_client.call_hand_service(0.0, 2.0) || !this->franka_ok){ ROS_ERROR("Could not open the hand."); res.success = false; @@ -525,4 +525,4 @@ bool TaskSequencer::call_set_object(panda_softhand_control::set_object::Request ROS_INFO_STREAM("Grasp pose changed. Object set to " << req.object_name << "."); res.result = true; return res.result; -} \ No newline at end of file +} diff --git a/panda_softhand_description/configs/panda_soma.rviz b/panda_softhand_description/configs/panda_soma.rviz index efc64b7..51e0eea 100644 --- a/panda_softhand_description/configs/panda_soma.rviz +++ b/panda_softhand_description/configs/panda_soma.rviz @@ -7,7 +7,7 @@ Panels: - /TF1 - /Trajectory1 Splitter Ratio: 0.470842332 - Tree Height: 689 + Tree Height: 660 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -106,6 +106,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Value: true right_hand_ee_link: Alpha: 1 Show Axes: false @@ -355,6 +356,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Value: true right_hand_ee_link: Alpha: 1 Show Axes: false @@ -546,6 +548,8 @@ Visualization Manager: Value: false panda_K: Value: false + panda_NE: + Value: true panda_link0: Value: false panda_link1: @@ -658,9 +662,10 @@ Visualization Manager: panda_link6: panda_link7: panda_link8: - panda_EE: - panda_K: - {} + panda_NE: + panda_EE: + panda_K: + {} right_hand_ee_link: {} right_hand_panda_flange: @@ -770,6 +775,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Value: true right_hand_ee_link: Alpha: 1 Show Axes: false @@ -1011,7 +1017,7 @@ Window Geometry: Height: 1056 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000017800000396fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000034b000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000379000000450000004300fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007470000003efc0100000002fb0000000800540069006d00650100000000000007470000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004b40000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001870000038afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e00000334000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000003690000004f0000004c00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005000ffffff00000001000001110000038afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002e0000038a000000b700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074700000040fc0100000002fb0000000800540069006d00650100000000000007470000038300fffffffb0000000800540069006d00650100000000000004500000000000000000000004a10000038a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RvizVisualToolsGui: collapsed: false Selection: @@ -1025,5 +1031,5 @@ Window Geometry: Views: collapsed: false Width: 1863 - X: 57 - Y: 1104 + X: 1977 + Y: 24 diff --git a/panda_softhand_description/launch/launchPandaSoftHand.launch b/panda_softhand_description/launch/launchPandaSoftHand.launch index 79acd84..3be6ff3 100644 --- a/panda_softhand_description/launch/launchPandaSoftHand.launch +++ b/panda_softhand_description/launch/launchPandaSoftHand.launch @@ -8,7 +8,7 @@ - + @@ -36,7 +36,7 @@ - + @@ -56,8 +56,8 @@ - - + + [franka_state_controller/joint_states, franka_gripper/joint_states] @@ -89,12 +89,12 @@ - + - + - + diff --git a/panda_softhand_description/worlds/panda_soma.world b/panda_softhand_description/worlds/panda_soma.world index ca479f4..808d5a5 100755 --- a/panda_softhand_description/worlds/panda_soma.world +++ b/panda_softhand_description/worlds/panda_soma.world @@ -2,16 +2,16 @@ - + - + + - 0.6 0 -0.05 0 0 0 + 0.65 0 -0.05 0 0 0 true 0 0 0 0 0 0 - + 0 0 0 0 0 0 @@ -49,7 +49,7 @@ - + @@ -57,19 +57,19 @@ - - 0 + + 0 1 - + - + - + - + - + diff --git a/panda_softhand_moveit_config/config/panda_arm.xacro b/panda_softhand_moveit_config/config/panda_arm.xacro index c0c5433..e2a9fc5 100644 --- a/panda_softhand_moveit_config/config/panda_arm.xacro +++ b/panda_softhand_moveit_config/config/panda_arm.xacro @@ -43,6 +43,7 @@ + @@ -220,6 +221,30 @@ + + + + + + + + + + + + + + + + + + + + + + + +