«Это первый день моего участия в первом испытании обновлений 2022 года. Подробную информацию о мероприятии см.:Вызов первого обновления 2022 г.".
Эта статья о том, как использовать программирование на C++ для захвата и размещения объектов. В программировании MoveIt наиболее часто используемым классом интерфейса является MoveGroupInterface. Класс MoveGroupInterface обеспечивает общие базовые операции роботов: установка углов сочленения, целевые позы робота, планирование пути и управление движением робота.
MoveGroupInterface взаимодействует с узлами MoveGroup через темы, службы и действия ROS.
moveit_msgs:: Хватка Соответствующие поля сообщения: trajectory_msgs/JointTrajectory pre_grasp_posture — это определяет положение траектории суставов в группе конечных эффекторов, прежде чем мы сделаем мастеринг.
trajectory_msgs/JointTrajectory grip_posture — определяет положение траектории суставов в группе конечных эффекторов для захвата объектов.
геометрия_msgs/PoseStamped grip_pose — Положение конечного эффектора, который должен быть захвачен.
moveit_msgs/GripperTranslation pre_grasp_approach — используется для определения направления и расстояния перемещения приближающегося объекта.
moveit_msgs/GripperTranslation post_grasp_retreat — используется для определения направления и расстояния перемещения после захвата объекта.
moveit_msgs/GripperTranslation post_place_retreat — используется для определения направления движения и пройденного расстояния, когда объект помещается в определенную позицию.
Создайте среду
Создайте вектор для удержания объектов 3 столкновения.
std::vector<moveit_msgs::CollisionObject> collision_objects;collision_objects.resize(3);
Добавьте первую таблицу, которая изначально будет содержать куб.
collision_objects[0].id = "table1";
collision_objects[0].header.frame_id = "panda_link0";
/* Define the primitive and its dimensions. */
collision_objects[0].primitives.resize(1);
collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
collision_objects[0].primitives[0].dimensions.resize(3);
collision_objects[0].primitives[0].dimensions[0] = 0.2;
collision_objects[0].primitives[0].dimensions[1] = 0.4;
collision_objects[0].primitives[0].dimensions[2] = 0.4;
/* Define the pose of the table. */
collision_objects[0].primitive_poses.resize(1);
collision_objects[0].primitive_poses[0].position.x = 0.5;
collision_objects[0].primitive_poses[0].position.y = 0;
collision_objects[0].primitive_poses[0].position.z = 0.2;
Добавьте второй стол, где мы будем размещать куб.
collision_objects[1].id = "table2";
collision_objects[1].header.frame_id = "panda_link0";
/* Define the primitive and its dimensions. */
collision_objects[1].primitives.resize(1);
collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[1].primitives[0].dimensions.resize(3);
collision_objects[1].primitives[0].dimensions[0] = 0.4;
collision_objects[1].primitives[0].dimensions[1] = 0.2;
collision_objects[1].primitives[0].dimensions[2] = 0.4;
/* Define the pose of the table. */
collision_objects[1].primitive_poses.resize(1);
collision_objects[1].primitive_poses[0].position.x = 0;
collision_objects[1].primitive_poses[0].position.y = 0.5;
collision_objects[1].primitive_poses[0].position.z = 0.2;
определить объект, над которым мы будем работать
collision_objects[2].header.frame_id = "panda_link0";
collision_objects[2].id = "object";
/* Define the primitive and its dimensions. */
collision_objects[2].primitives.resize(1);
collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[2].primitives[0].dimensions.resize(3);
collision_objects[2].primitives[0].dimensions[0] = 0.02;
collision_objects[2].primitives[0].dimensions[1] = 0.02;
collision_objects[2].primitives[0].dimensions[2] = 0.2;
/* Define the pose of the object. */
collision_objects[2].primitive_poses.resize(1);
collision_objects[2].primitive_poses[0].position.x = 0.5;
collision_objects[2].primitive_poses[0].position.y = 0;
collision_objects[2].primitive_poses[0].position.z = 0.5;
Возьмите трубопровод Создайте вектор захватов, чтобы попробовать, в настоящее время создается только один захват. Это в основном полезно при использовании генератора захвата для создания и тестирования нескольких захватов.
std::vector<moveit_msgs::Grasp> grasps;grasps.resize(1);
Это поза panda_link8.
От panda_link8 до пальмы говядины расстояние 0,058, а куб начинается с 0,01 до 5,0 (половина длины куба).
Итак, позиция panda_link8 = 5 - (длина куба / 2 - расстояние ч/б отпечатка ладони panda_link8 и отпечатка ладони - дополнительное дополнение)
grasps[0].grasp_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
grasps[0].grasp_pose.pose.position.x = 0.415;
grasps[0].grasp_pose.pose.position.y = 0;
grasps[0].grasp_pose.pose.position.z = 0.5;
Установите путь перед очисткой
/* Defined with respect to frame_id */
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as positive x axis */
grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
grasps[0].pre_grasp_approach.min_distance = 0.095;
grasps[0].pre_grasp_approach.desired_distance = 0.115;
Установить обратный маршрут
/* Defined with respect to frame_id */
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as positive z axis */
grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
grasps[0].post_grasp_retreat.min_distance = 0.1;
grasps[0].post_grasp_retreat.desired_distance = 0.25;
Setting posture of eef before grasp
openGripper(grasps[0].pre_grasp_posture);
openGripper function
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.04;
posture.points[0].positions[1] = 0.04;
posture.points[0].time_from_start = ros::Duration(0.5);
Setting posture of eef during grasp
closedGripper(grasps[0].grasp_posture);
closedGripper function
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.00;
posture.points[0].positions[1] = 0.00;
posture.points[0].time_from_start = ros::Duration(0.5);
Установите опорную поверхность на table1.
move_group.setSupportSurfaceName("table1");
Используйте данный захват, чтобы вызвать сборщик, чтобы забрать объект
move_group.pick("object", grasps);
TODO (@ridhwanluthra) - Вызов функции мест проведения может привести к ошибке "Не удалось найти все указанные места проведения. Повторная попытка указать последнее место в подробном режиме". Это известная проблема, и мы работаем над ее устранением.
Создайте вектор позиций, чтобы попробовать, в настоящее время создается только одна позиция.
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
установить позу местоположения
place_location[0].place_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(0, 0, M_PI / 2);
place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */
place_location[0].place_pose.pose.position.x = 0;
place_location[0].place_pose.pose.position.y = 0.5;
place_location[0].place_pose.pose.position.z = 0.5;
Setting place location pose
place_location[0].place_pose.header.frame_id = "panda_link0";tf2::Quaternion orientation;orientation.setRPY(0, 0, M_PI / 2);place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */place_location[0].place_pose.pose.position.x = 0;place_location[0].place_pose.pose.position.y = 0.5;place_location[0].place_pose.pose.position.z = 0.5;
Прямой маршрут
/* Defined with respect to frame_id /place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";/ Direction is set as negative z axis */place_location[0].pre_place_approach.direction.vector.z = -1.0;place_location[0].pre_place_approach.min_distance = 0.095;place_location[0].pre_place_approach.desired_distance = 0.115;
Маршрут отступления после захвата
/* Defined with respect to frame_id /place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";/ Direction is set as negative y axis */place_location[0].post_place_retreat.direction.vector.y = -1.0;place_location[0].post_place_retreat.min_distance = 0.1;place_location[0].post_place_retreat.desired_distance = 0.25;
Setting posture of eef after placing object
/* Similar to the pick case */openGripper(place_location[0].post_place_posture);
Set support surface as table2.
group.setSupportSurfaceName("table2");
Call place to place the object using the place locations given.
group.place("object", place_location);