How to set start state in moveit except setStartState()
I do not want my robot plan from current pose. So I try to set start pose of my robot to another pose.
I followed http://docs.ros.org/indigo/api/moveit... , and used setStartState() to change robot start pose.
robot_state::RobotState start_state(*group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
const robot_state::JointModelGroup *joint_model_group =
start_state.getJointModelGroup(group.getName());
start_state.setFromIK(joint_model_group, start_pose2);
group.setStartState(start_state);
But, I found setFromIK() always return false and could not find IK solution.
Is there any problem in this program? Or is there other method to set start pose of robot? Thank you very much.