How can I set a robot_state::RobotState to a given pose (not given joint value)?

[唐威]

Without using plan and execute from movegroup, Can I set the robotstate to arbitrary pose so that I can detect the collision at that state?

Thank you in advance

Best,

Wei

[Simon Schmeißer]

Sure, you need a PlanningScene:

planning_scene_monitor::LockedPlanningSceneRW ps(m_planningSceneMonitor);
ps->getCurrentStateNonConst().update();

now use ps->getCurrentState() to get a RobotState

use the functions of RobotState to modify it.

use functions of PlanningScene to check for collisions of this RobotState

[唐威]

Thank you for your answer! I think I 've learned a lot from you from your old answers. Could you have a look at the following code? The beginning is some basic setup like you said. 

The confusing part is the last 4 lines. It seems the setFromIk doesn't do any change to the robotState. Also, there are move_group and rviz node running in the meanwhile, how can I 

let the rviz change according to the robotState I changed here? 

Best,

Wei 

 planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);

  ps->getCurrentStateNonConst().update();

  //if you want to modify it

  planning_scene::PlanningScenePtr scene = ps->diff();

  scene->decoupleParent();  

  robot_model::RobotModelConstPtr robot_model = scene->getRobotModel();

  const robot_state::JointModelGroup* joint_model_group = robot_model->getJointModelGroup("ur5");

  geometry_msgs::Pose ee_pose;

  ee_pose.position.x = 1;

  ee_pose.position.y = 1;

  ee_pose.position.z = 3;

  ee_pose.orientation.x = 0;

  ee_pose.orientation.y = 0;

  ee_pose.orientation.z = 0;

  ee_pose.orientation.w = 1;

  robot_state::RobotState current_state = scene->getCurrentStateNonConst();   

  current_state.setToDefaultValues();

  current_state.printStateInfo(std::cout);

  current_state.setFromIK(joint_model_group, ee_pose, 5, 0.1);

  current_state.printStateInfo(std::cout);

在 2016年2月26日星期五 UTC+8下午6:06:16，Simon Schmeißer写道：

[Simon Schmeißer]

maybe your pose cannot be reached? Check the return value of setFromIK.
http://docs.ros.org/jade/api/moveit_core/html/classmoveit_1_1core_1_1RobotState.html#ab816880027ef7e63bbdef22a0497cc78

Also note that there is no collision checking unless you pass in a function pointer to a collision checking function

If you want to move the robot, there are move_group functions for moving either to a pose or to a joint configuration. If you just want to display your calculation you can publish RobotStateMsg and add a robot state plugin in rviz to display it.