Hello guys,
I want to get my Universal Robot model keep level in the simulation in Moveit which means the rotation of X axis and Y axis of the robot's end-effector will not be changed and the the rotation of Z axis can be controlled by the planner. My code is listed as followed
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);
// BEGIN_TUTORIAL
//
// Setup
// ^^^^^
//
// The :move_group_interface:`MoveGroup` class can be easily
// setup using just the name
// of the group you would like to control and plan for.
moveit::planning_interface::MoveGroup group("arm");
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to deal directly with the world.
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// (Optional) Create a publisher for visualizing plans in Rviz.
ros::Publisher display_publisher = node_handle.advertise("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
// We can print the name of the reference frame for this robot.
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
robot_state::RobotState start_state(*group.getCurrentState());
geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
//geometry_msgs::Pose start_pose2;
group.setStartState(start_state);
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "tt_base_link";
ocm.header.frame_id = "base_link";
ocm.orientation.x = 0.003;
ocm.orientation.y = 0.002;
//ocm.orientation.z = 0.101;
//ocm.orientation.w = 0.949;
//ocm.orientation.z = 0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
// Now, set it as the path constraint for the group.
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
group.setPathConstraints(test_constraints);
std::vector waypoints;
geometry_msgs::Pose target_pose1;
//ocm.orientation.x = 0;
//ocm.orientation.y = 0;
// ocm.orientation.x = 0.003;
//ocm.orientation.y = 0.002;
//ocm.orientation.z = 0.101;
// ocm.orientation.w = 0.949;
target_pose1.position.x = 0.53;
target_pose1.position.y = 0.24;
target_pose1.position.z = 0.81;
waypoints.push_back(target_pose1);
moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
// Now we will plan to the earlier pose target from the new
// start state that we have just created.
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);
ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);
// When done with the path constraint be sure to clear it.
group.clearPathConstraints();
ros::shutdown();
return 0;
}
So could you check the code and tell me about the mistakes?
---
Edit: @v4hn
Thanks for your answer. I have used the `computeCartesianPath` and `setPathConstraints`to constraint the orientation and let the robot move to a point `next_pose`. And the robot's orientation x,y,z,w did not changed in the process of moving. But my target is that pitch angle and roll angle of the end effector equaling 0 to keep it level and the yaw angle can be any value that is planned by Moveit and now these three angles are all equaling 0 (yaw angle did not change). So could you tell me about the problem and my codes are listed.
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "tool0";
ocm.header.frame_id = "base_link";
ocm.orientation.w = 0.556;
ocm.absolute_x_axis_tolerance = 0.0;
ocm.absolute_y_axis_tolerance = 0.0;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
group_r.setPathConstraints(test_constraints);
geometry_msgs::Pose next_pose = start_pose;
std::vector waypoints;
next_pose.position.x=0.626;
next_pose.position.y=0.129;
next_pose.position.z=0.545;
//next_pose.orientation.x = 0.002;
//next_pose.orientation.y = 0.00045;
//next_pose.orientation.z = 0.830;
//next_pose.orientation.w = 0.556;
//next_pose.position.y += 0.5;
//next_pose.position.z += 0.3;
//next_pose.orientation.w = 0.557;
waypoints.push_back(next_pose); // up and out
moveit_msgs::RobotTrajectory trajectory;
double fraction = group_r.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);
↧