Hello
I am trying to keep the end effector of the UR10 arm parallel to the ground using Moveit, which means the pose of end effector's pitch=0, roll=0 and yaw angle can be any value. My code is shown as followed and the motion plan can't be found, `Fail: ABORTED: No motion plan found. No execution attempted,`
. And i am sure the pose 'next_pose" is reachable. Would you mind checking the code and pointing out the mistake?
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
/* This sleep is ONLY to allow Rviz to come up */
sleep(20.0);
moveit::planning_interface::MoveGroup group_r("arm");
group_r.setPlannerId("RRTConnectkConfigDefault");
group_r.setPlanningTime(45);
//ocm
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "tt_base_link";
ocm.header.frame_id = "base_link";
ocm.orientation.x = 0.210738;
ocm.orientation.y = 0.672778;
ocm.orientation.z = 0.677635;
ocm.orientation.w = 0.209212;
ocm.absolute_x_axis_tolerance = 0.01;
ocm.absolute_y_axis_tolerance = 0.01;
ocm.absolute_z_axis_tolerance = 3.14;
ocm.weight = 1.0;
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
group_r.setPathConstraints(test_constraints);
//start pose
geometry_msgs::Pose start_pose;
start_pose.position.x = -0.402141;
start_pose.position.y = 0.162843;
start_pose.position.z = 0.384870;
start_pose.orientation.x = -0.003270;
start_pose.orientation.y = 0.704558;
start_pose.orientation.z = 0.709631;
start_pose.orientation.w = -0.003249;
const robot_state::JointModelGroup *joint_model_group =
//start_state.getJointModelGroup(group_r.getName());
start_state.setFromIK(joint_model_group, start_pose);
group_r.setStartState(start_state);
// target
geometry_msgs::Pose next_pose;
next_pose.position.x =-0.402141;
next_pose.position.y =0.462843;
next_pose.position.z =0.384870;
next_pose.orientation.x =0.167744;
next_pose.orientation.y =0.684582;
next_pose.orientation.z =0.689516;
next_pose.orientation.w =0.166666;
group_r.setPoseTarget(next_pose,"tt_base_link");
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group_r.plan(my_plan);
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
sleep(5.0);
group_r.clearPathConstraints();
}
↧