ROS Moveit ограничения
Я пытаюсь использовать его для перемещения ТОЛЬКО вертикально. Идея состоит в том, чтобы сохранить кончик эффектора так, чтобы всегда сохранять положение оси x и y и изменять положение оси z в каждой итерации, сохраняя при этом свою ориентацию. Я также хочу ограничить движение от начальной позиции к конечной позиции в каждой итерации, чтобы следовать этим ограничениям (фиксированные значения x и y, только изменение z). Меня не волнует, насколько сильно меняются суставы, пока захват (мой концевой эффектор) движется только вверх.
Я попытался сделать это, как показано ниже, но я не вижу никаких ограничений, которым следуют? Что я делаю неправильно?
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("right_arm");
robot_state::RobotState start_state(*group_r.getCurrentState());
geometry_msgs::Pose start_pose;
start_pose.orientation.x = group_r.getCurrentPose().pose.orientation.x;
start_pose.orientation.y = group_r.getCurrentPose().pose.orientation.y;
start_pose.orientation.z = group_r.getCurrentPose().pose.orientation.z;
start_pose.orientation.w = group_r.getCurrentPose().pose.orientation.w;
start_pose.position.x = group_r.getCurrentPose().pose.position.x;
start_pose.position.y = group_r.getCurrentPose().pose.position.y;
start_pose.position.z = group_r.getCurrentPose().pose.position.z;
//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);
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = "base_link";
ocm.orientation.w = 0.0;
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;
while(1){
std::vector<geometry_msgs::Pose> waypoints;
next_pose.position.z -= 0.03;
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);
}}
Решение
Я считаю, что проблема заключается в следующем:
ocm.orientation.w = 0.0;
Если вы посмотрите на moveit_msgs::OrientationConstraint ссылка вы найдете интерпретацию этого orientation поле.
# The desired orientation of the robot link specified as a quaternion
geometry_msgs/Quaternion orientation
Однако вы устанавливаете все поля кватерниона в 0 (мнимые x, y и z инициализируются 0, если не указано), что может вызвать неожиданное поведение.
Если вы следовали этот урок, Вы могли видеть, что автор установил ocm.orientation.w = 1.0; что означает отсутствие изменения ориентации (то есть угол наклона и угол рыскания равны 0). Таким образом, попробуйте указать реалистичную ориентацию для вашего ограничения.
И последнее, но не менее важное: для ясности было бы лучше написать start_pose инициализации:
geometry_msgs::Pose start_pose = group_r.getCurrentPose().pose;
Другие решения