how to set orientation constraints to my end effector?
Hi all,
I would like my UR5's end-effector to have an orientation constraint. Here is the code I used to try to achieve that:
//Adding Path Constraints
planning_interface::MotionPlanRequest req;
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
geometry_msgs::PoseStamped pose;
moveit_msgs::Constraints target_pose =
kinematic_constraints::constructGoalConstraints("wrist_3_joint", pose, tolerance_pose, tolerance_angle);
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "ee_link"; //"wrist_3_link";
quaternion.quaternion.x = 0;
quaternion.quaternion.y = 0;
quaternion.quaternion.z = 0;
quaternion.quaternion.w = 1;
req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_joint", quaternion);
I am pretty sure there is something wrong in this code, because the end effector sometimes changes his orientation during the execution of the plan.
Any help?
ROS kinetic, ubuntu 16.04
edit: full code here:
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/kinematic_constraints/utils.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_move_group");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "manipulator";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("world");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
visual_tools.loadRemoteControl();
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.10);
move_group.setPlanningTime(0.1);
move_group.allowReplanning(true);
move_group.setPlannerId("RRTConnectkConfigDefault");
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1;
target_pose1.position.x = 0.50;
target_pose1.position.y = 0.00;
target_pose1.position.z = 0.34;
geometry_msgs::Pose target_pose2;
target_pose2.orientation.w = 1;
target_pose2.position.x = 0.00;
target_pose2.position.y = 0.50;
target_pose2.position.z = 0.34;
// Adding Path Constraints
planning_interface::MotionPlanRequest req;
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
geometry_msgs::PoseStamped pose;
moveit_msgs::Constraints target_pose =
kinematic_constraints::constructGoalConstraints("ee_link", pose, tolerance_pose, tolerance_angle);
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "base_link";
quaternion.quaternion.x = 0;
quaternion.quaternion.y = 0;
quaternion.quaternion.z = 0;
quaternion.quaternion.w = 1;
req.path_constraints = kinematic_constraints::constructGoalConstraints ("wrist_3_joint", quaternion);
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
req.workspace_parameters.max_corner.z = 2.0;
while (ros::ok()) {
visual_tools.prompt("Press 'next'");
move_group.setPoseTarget(target_pose1);
move_group.asyncMove();
move_group.getCurrentState();
visual_tools.prompt("Press 'next'");
move_group.setPoseTarget(target_pose2);
move_group.asyncMove();
move_group.getCurrentState();
}
ros::shutdown();
return 0;
}
Hello! I don't know your robot, but the
quaternion.header.frame_id
parameter is usually the robot base. Is "ee_link" your robot base? Source: "Motion Planning API" tutorial, section "Entire Code", subsection "Adding Path"Yeah you're right, but even with "base_link" it still does not always work. Maybe it's a matter of planning time too short?
I am a beginner using MoveIt, but I doubt it is a planning time problem. One more thing different in the tutorial is:
constructGoalConstraints
first argument is not a joint but a link. Also, where do you set your target_pose? I only see declaration of variable, not of position and orientation.You can take a look at my full code as I just edited the question. If you notice something weird don't hesitate to tell me, I am also a beginner of Moveit (and C++)
I think using asyncMove() is not a good idea in this context. How can it take into account the
MotionPlanRequest
you created if it is not part of its arguments nor a member of move_group class? I think it is better to use the same method as the tutorial (explicit call to planner withsolve
)I will try at home and get back to you. Sorry for not helping much
I'm really confused. Are you simulating the robot or using a real one? If simulating, where in your code do you load the robot model? Where do you load visual tools? I have to admit that your code is too different from the tutorial for me to be able to help, I'm just not good enough with MoveIt.
Hi, thanks for your continuous support. I am using a real robot with a position controller (ur5_ros_control). What do you mean when you ask: "where do you load visual tools?"