int move_arm_out_of_the_way() {
//set_joint_goal();
//clear_collision_map();
ROS_INFO("Move arm out of the way.");
//move arm to initial home state as defined in the urdf
group->setNamedTarget("home_stable");
group->asyncMove();
//clear_collision_map();
ROS_INFO("Arm moved out of the way.");
return 1;
}
void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
//ROS_ERROR_STREAM("move_to_wait_position is not implemented yet. Aborting."); exit(1);
// task variables
bool success; // saves the move result
// set robot wait target
/* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
move_group.setNamedTarget(cfg.WAIT_POSE_NAME);
// move the robot
/* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
success = move_group.move();
if(success)
{
ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
}
else
{
ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
exit(1);
}
}