Panda机械臂

环境构建

1. 源码下载

git clone -b melodic-devel https://github.com/ros-planning/panda_moveit_config.git

2. 依赖安装

sudo apt install ros-melodic-franka-ros
cd ws/src
rosdep install -y --from-paths . --ignore-src --rosdistro melodic

3. 启动可视化环境

roslaunch panda_moveit_config demo.launch

案例流程

添加物体

1. 添加桌子1

std::vector<moveit_msgs::CollisionObject> objects;

// 第一个桌子
moveit_msgs::CollisionObject obj;
obj.id = "desk01";
obj.operation = obj.ADD;
obj.header.frame_id = frame;

//桌子形状
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;//矩形，长宽高
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.2;
primitive.dimensions[1] = 0.4;
primitive.dimensions[2] = 0.4;
obj.primitives.push_back(primitive);

//桌子的位姿
geometry_msgs::Pose pose;
pose.position.x = 0.5;
pose.position.y = 0;
pose.position.z = 0.2;
pose.orientation.w = 1;
obj.primitive_poses.push_back(pose);

objects.push_back(obj);
scene.addCollisionObjects(objects);

1. 添加桌子2

std::vector<moveit_msgs::CollisionObject> objects;
// 第二个桌子
moveit_msgs::CollisionObject obj;
obj.id = "desk02";
obj.operation = obj.ADD;
obj.header.frame_id = frame;

//桌子形状
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;//矩形，长宽高
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.2;
primitive.dimensions[2] = 0.4;
obj.primitives.push_back(primitive);

//桌子的位姿
geometry_msgs::Pose pose;
pose.position.x = 0;
pose.position.y = 0.5;
pose.position.z = 0.2;
pose.orientation.w = 1;
obj.primitive_poses.push_back(pose);

objects.push_back(obj);
scene.addCollisionObjects(objects);

1. 添加夹取物

std::vector<moveit_msgs::CollisionObject> objects;
// 添加物体
moveit_msgs::CollisionObject obj;
obj.id = "obj";
obj.operation = obj.ADD;
obj.header.frame_id = frame;

//物体形状
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;//矩形，长宽高
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.02;
primitive.dimensions[1] = 0.02;
primitive.dimensions[2] = 0.2;
obj.primitives.push_back(primitive);

//物体位姿
geometry_msgs::Pose pose;
pose.position.x = 0.5;
pose.position.y = 0;
pose.position.z = 0.5;
pose.orientation.w = 1;
obj.primitive_poses.push_back(pose);

objects.push_back(obj);
scene.addCollisionObjects(objects);

夹取物体

1. 夹取物品

std::vector<moveit_msgs::Grasp> grasps;
moveit_msgs::Grasp grasp;
...
grasps.push_back(grasp);
group.pick("obj", grasps);

1. 设置夹爪的位姿

//设置位姿参考坐标系
grasp.grasp_pose.header.frame_id = group.getPlanningFrame();
//设置位姿
grasp.grasp_pose.pose.position.x = 0.4;
grasp.grasp_pose.pose.position.y = 0;
grasp.grasp_pose.pose.position.z = 0.5;
tf::Quaternion quat;
quat.setRPY(-1.57, -0.785, -1.57);
grasp.grasp_pose.pose.orientation.x = quat.x();
grasp.grasp_pose.pose.orientation.y = quat.y();
grasp.grasp_pose.pose.orientation.z = quat.z();
grasp.grasp_pose.pose.orientation.w = quat.w();

1. 夹取前移动

//夹取前，沿着x轴方向靠近
grasp.pre_grasp_approach.direction.header.frame_id = group.getPlanningFrame();
grasp.pre_grasp_approach.direction.vector.x = 1.0;
grasp.pre_grasp_approach.min_distance = 0.095;
grasp.pre_grasp_approach.desired_distance = 0.115;

1. 打开夹爪

//打开夹爪
grasp.pre_grasp_posture.joint_names.resize(2);
grasp.pre_grasp_posture.joint_names[0] = "panda_finger_joint1";
grasp.pre_grasp_posture.joint_names[1] = "panda_finger_joint2";
// 两个手指的移动（0-0.04）
grasp.pre_grasp_posture.points.resize(1);
grasp.pre_grasp_posture.points[0].positions.resize(2);
grasp.pre_grasp_posture.points[0].positions[0] = 0.04;
grasp.pre_grasp_posture.points[0].positions[1] = 0.04;
grasp.pre_grasp_posture.points[0].time_from_start = ros::Duration(0.5);

1. 关闭夹爪

//关闭夹爪
grasp.grasp_posture.joint_names.resize(2);
grasp.grasp_posture.joint_names[0] = "panda_finger_joint1";
grasp.grasp_posture.joint_names[1] = "panda_finger_joint2";
// 两个手指的移动（0-0.04）
grasp.grasp_posture.points.resize(1);
grasp.grasp_posture.points[0].positions.resize(2);
grasp.grasp_posture.points[0].positions[0] = 0.01;
grasp.grasp_posture.points[0].positions[1] = 0.01;
grasp.grasp_posture.points[0].time_from_start = ros::Duration(0.5);

1. 夹取后移动

//夹取后,沿着Z轴方向离开
grasp.post_grasp_retreat.direction.header.frame_id = group.getPlanningFrame();
grasp.post_grasp_retreat.direction.vector.z = 1.0;
grasp.post_grasp_retreat.min_distance = 0.1;
grasp.post_grasp_retreat.desired_distance = 0.25;

放置物体

1. 放置物体

// 放置物体
std::vector<moveit_msgs::PlaceLocation> places;
moveit_msgs::PlaceLocation place;
...
places.push_back(place);
group.place("obj",places);

1. 设置夹爪位姿

place.place_pose.header.frame_id = group.getPlanningFrame();
place.place_pose.pose.position.x = 0;
place.place_pose.pose.position.y = 0.5;
place.place_pose.pose.position.z = 0.5;
tf::Quaternion quat;
quat.setRPY(0, 0, 1.57);
place.place_pose.pose.orientation.x = quat.x();
place.place_pose.pose.orientation.y = quat.y();
place.place_pose.pose.orientation.z = quat.z();
place.place_pose.pose.orientation.w = quat.w();

1. 放置前移动

//沿着y轴接近放置物体的目标
place.pre_place_approach.direction.header.frame_id = group.getPlanningFrame();
place.pre_place_approach.direction.vector.y = 1.0;
place.pre_place_approach.min_distance = 0.095;
place.pre_place_approach.desired_distance = 0.115;

1. 打开夹爪

place.post_place_posture.joint_names.resize(2);
place.post_place_posture.joint_names[0] = "panda_finger_joint1";
place.post_place_posture.joint_names[1] = "panda_finger_joint2";
// 两个手指的移动（0-0.04）
place.post_place_posture.points.resize(1);
place.post_place_posture.points[0].positions.resize(2);
place.post_place_posture.points[0].positions[0] = 0.04;
place.post_place_posture.points[0].positions[1] = 0.04;
place.post_place_posture.points[0].time_from_start = ros::Duration(0.5);

1. 放下物体

// 放下物体
place.post_place_retreat.direction.header.frame_id = group.getPlanningFrame();
place.post_place_retreat.direction.vector.z = 1.0;
place.post_place_retreat.min_distance = 0.1;
place.post_place_retreat.desired_distance = 0.25;

1. 关闭夹爪

//关闭夹爪
place.post_place_posture.joint_names.resize(2);
place.post_place_posture.joint_names[0] = "panda_finger_joint1";
place.post_place_posture.joint_names[1] = "panda_finger_joint2";
// 两个手指的移动（0-0.04）
place.post_place_posture.points.resize(1);
place.post_place_posture.points[0].positions.resize(2);
place.post_place_posture.points[0].positions[0] = 0.01;
place.post_place_posture.points[0].positions[1] = 0.01;
place.post_place_posture.points[0].time_from_start = ros::Duration(0.5);