How to move base robot in MoveIt! hydro

asked 2014-06-03 11:43:44 -0500

Fran Marquez gravatar image

updated 2015-04-30 01:10:45 -0500

130s gravatar image

Hi everyone!

I am trying to do a motion planning with a quadcopter with an arm manipulator. I am using the MoveGroup interface in Moveit! For that, I run my demo.launch created by moveit_setup_assistant and file file .cpp with the code that I did for the motion.

I can see the base motion in Rviz but the base of quadcopter really don't move, thus I can not make motions with the manipulator from the last position of the base motion. I see first the motion of the base, and after I see the manipulator motion form the first place. Moveit! don't save the base motion. what is the problem?

Here is the code:

moveit::planning_interface::MoveGroup quad("whole_robot");  
moveit::planning_interface::MoveGroup group("arm");
//only manipulator

 std::vector<double> quad_variable_values;
quad.getCurrentState()->copyJointGroupPositions(quad.getCurrentState()->getRobotModel()->getJointModelGroup(quad.getName()), quad_variable_values);
quad_variable_values[2] = 0.69;
//This is position and orientation for the quadcopter

moveit::planning_interface::MoveGroup::Plan my_plan;
bool  success = quad.plan(my_plan);


  quad.getCurrentState()->copyJointGroupPositions(quad.getCurrentState()->getRobotModel()->getJointModelGroup(quad.getName()), quad_variable_values);
ROS_INFO("Variables articulares %f %f %f %f %f %f %f",quad_variable_values[0],quad_variable_values[1],quad_variable_values[2],quad_variable_values[3],quad_variable_values[4],quad_variable_values[5],quad_variable_values[6]);
  group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), variable_values);

//I copy the joint positions in quad_variable_values and show it.

//And I do the same for the manipulator
 std::vector<double> group_variable_values;

  group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
  group_variable_values[1] = -1;
  group_variable_values[2] = 0;
     moveit::planning_interface::MoveGroup::Plan my_plan2;
 success = group.plan(my_plan2);

The result is:

[ INFO] [1401795116.063443488]: Ready to take MoveGroup commands for group arm.
[ INFO] [1401795116.627121832]: Ready to take MoveGroup commands for group whole_robot.
[ INFO] [1401795120.966969193]: Variables articulares 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000

Thanks for your help!

edit retag flag offensive close merge delete


Hello, did you check topics and frames? $rostopic list and $rosrun tf view_frames. It seems to be your problem

est_CEAR gravatar image est_CEAR  ( 2014-11-18 02:07:03 -0500 )edit