How to choose move_group("arm") and move_group("arm_with_torso")?

[mptangtang]

Following the tutorial , it is easy to move the torso of fetch using move_group("arm_with_torso") in joint space.

However, if I firstly would like to raise the torso to some prefer height then fix the torso to control the arm only. What should I do? I have tried two move_groups.

When using move_group("arm"), the torso would move back to zero immediately after the move_group.execute(plan) instructions.

If move_group("arm_with_torso") is used, if I set the goal pose to the move_group, then the fetch arm will move with torso joint.

What should I do in this situation?

Best wishes!

[erelson]

If I understand your intention correctly, it sounds like a workable approach would be to always use the arm_with_torso controller: (1) to raise the torso, first get the arm join positions and then set those positions so the arm is "locked"; (2) then lock the torso at the height it reaches, while you move the arm during the rest of the experiment.

This incomplete snippet I grabbed should give an idea how we get the arm positions in a test where we don't want the arm moving:

            arm_torso_state_msg = get_joint_state_msg()  # /joint_states topic
            # Create list of joints (which actually/still exist)
            self._armjoints = [joint for joint in ARM_JOINTS if joint in
                      arm_torso_state_msg.name]
            # Create list of current joint positions
            self._joint_positions = \
                    [arm_torso_state_msg.position[
                             arm_torso_state_msg.name.index(joint)]
                     for joint in self._armjoints]

It does sound odd that the torso moves back to an old position when doing move_group("arm")... FYI @moriarty

[mptangtang]

This is my test code. I check the torso's position before and after the execution. However it turns out that the torso joint suddently drop to zero after the execution. #include <ros/ros.h> #include <moveit/move_group_interface/move_group_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit_visual_tools/moveit_visual_tools.h> void print_joint_value(unsigned int joint_index, double joint_value){ ROS_INFO("Joint %d's value is %f. ", joint_index, joint_value); } int main(int argc, char **argv) { ros::init(argc, argv, "fetch_moveit_switch_move_group"); ros::NodeHandle nh; ///////////////////////////////////////////////////// // Start ROS spinner ros::AsyncSpinner spinner(1); spinner.start(); ///////////////////////////////////////////////////// // MoveIt operates on sets of joints called "planning groups" and stores them in an object called // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group" // are used interchangably. static const std::string ARM_TORSO_PLANNING_GROUP = "arm_with_torso"; // arm torso static const std::string ARM_PLANNING_GROUP = "arm"; // arm // The :move_group_interface:`MoveGroupInterface` class can be easily // setup using just the name of the planning group you would like to control and plan for. moveit::planning_interface::MoveGroupInterface arm_torso_group(ARM_TORSO_PLANNING_GROUP); moveit::planning_interface::MoveGroupInterface arm_group(ARM_PLANNING_GROUP); // Raw pointers are frequently used to refer to the planning group for improved performance. const robot_state::JointModelGroup* arm_torso_joint_model_group = arm_torso_group.getCurrentState()->getJointModelGroup(ARM_TORSO_PLANNING_GROUP); const robot_state::JointModelGroup* arm_joint_model_group = arm_group.getCurrentState()->getJointModelGroup(ARM_PLANNING_GROUP); // We can get a list of all the groups in the robot: ROS_INFO("-----------------------------------------------"); ROS_INFO_NAMED("tutorial_1", "Available Planning Groups1:"); std::copy(arm_torso_group.getJointModelGroupNames().begin(), arm_torso_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", ")); ROS_INFO("\n-----------------------------------------------"); ROS_INFO_NAMED("tutorial_2", "Available Planning Groups2:"); std::copy(arm_group.getJointModelGroupNames().begin(), arm_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", ")); ROS_INFO("\n-----------------------------------------------"); // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("base_link"); visual_tools.deleteAllMarkers(); visual_tools.trigger(); /////////////////////////////////////////////////////////// // Raise the torso for initialization of fetch std::vector<double> fetch_default_positions; fetch_default_positions.resize(8); fetch_default_positions[0] = 0.3; fetch_default_positions[1] = 1.319999; fetch_default_positions[2] = 1.399986; fetch_default_positions[3] = -0.199989; fetch_default_positions[4] = 1.719962; fetch_default_positions[5] = 0.000001; fetch_default_positions[6] = 1.660008; fetch_default_positions[7] = -0.000001; moveit::core::RobotStatePtr current_state = arm_torso_group.getCurrentState(); std::vector<double> current_group_positions; current_state->copyJointGroupPositions(arm_torso_joint_model_group, current_group_positions); ROS_INFO("---------------- Current Fetch Joint Values ----------------"); for(unsigned int i = 0; i<current_group_positions.size(); ++i){ print_joint_value(i, current_group_positions[i]); } arm_torso_group.setJointValueTarget(fetch_default_positions); moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (arm_torso_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); if(success){ arm_torso_group.execute(my_plan); ROS_INFO("Fetch move to initial position in joint space."); sleep(2.0); }else{ ROS_INFO("Planning fetch to initial joint space failed!"); return 0; } visual_tools.prompt("Initialize fetch to the default position. \n" "Press 'next' in the RvizVisualToolsGui window to continue the next movement."); ////////////////////////////////////////////////////////////////////////////// /// \brief target_pose1 geometry_msgs::Pose target_pose1; target_pose1.orientation.w = 1.0; target_pose1.position.x = 0.28; target_pose1.position.y = -0.3; target_pose1.position.z = 1.3; current_state = arm_torso_group.getCurrentState(); current_state->copyJointGroupPositions(arm_torso_joint_model_group, current_group_positions); ROS_INFO("---------------- Current Fetch Joint Values ----------------"); for(unsigned int i = 0; i<current_group_positions.size(); ++i){ print_joint_value(i, current_group_positions[i]); } robot_state::RobotState start_state(*arm_group.getCurrentState()); start_state.setVariablePosition("torso_lift_joint", 0.3); start_state.update(true); //start_state.setToDefaultValues(); ROS_INFO("The torso's position is %f", start_state.getVariablePosition("torso_lift_joint")); arm_group.setStartState(start_state); ROS_INFO("After setting start state, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint")); arm_group.rememberJointValues("torso_lift_joint"); arm_group.setJointValueTarget("torso_lift_joint", 0.3); arm_group.setPoseTarget(target_pose1); ROS_INFO("After setting start state, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint")); moveit::planning_interface::MoveGroupInterface::Plan my_plan2; success = (arm_group.plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO("After setting start state, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint")); if(success){ // arm_group.move(); arm_group.execute(my_plan2); sleep(3.0); } ROS_INFO("After movement, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint")); return 0; } Eric Relson <[email protected]> 于2019年8月5日周一 下午11:03写道：

…

If I understand your intention correctly, it sounds like a workable approach would be to always use the arm_with_torso controller: (1) to raise the torso, first get the arm join positions and then set those positions so the arm is "locked"; (2) then lock the torso at the height it reaches, while you move the arm during the rest of the experiment. This incomplete snippet I grabbed should give an idea how we get the arm positions in a test where we don't want the arm moving: arm_torso_state_msg = get_joint_state_msg() # /joint_states topic # Create list of joints (which actually/still exist) self._armjoints = [joint for joint in ARM_JOINTS if joint in arm_torso_state_msg.name] # Create list of current joint positions self._joint_positions = \ [arm_torso_state_msg.position[ arm_torso_state_msg.name.index(joint)] for joint in self._armjoints] It does sound odd that the torso moves back to an old position when doing move_group("arm")... FYI @moriarty <https://github.com/moriarty> — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#71?email_source=notifications&email_token=AKHQT7SYDVDSIRGKQRDBNCLQDA6KNA5CNFSM4IJL2XKKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOD3SDFCY#issuecomment-518271627>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AKHQT7RJ5W3LK3AT6JTOAF3QDA6KNANCNFSM4IJL2XKA> .