机器人 笛卡尔运动规划
flyfish
#include <ros/ros.h>
// 包含moveit的API
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <eigen_conversions/eigen_msg.h>
#include <vector>
#include <tf/transform_broadcaster.h>
#include <vector>
int main(int argc, char **argv)
{
bool cartesian =true; //需要编写成参数形式
ros::init (argc, argv, "moveit_cartesian_demo",ros::init_options::AnonymousName);
ros::AsyncSpinner spinner(1);
spinner.start();
// ros::NodeHandle nodeHandle;
//初始化需要使用move group控制的机械臂中的arm group
moveit::planning_interface::MoveGroupInterface arm("arm");
ROS_INFO("Planning reference frame: %s", arm.getPlanningFrame().c_str());
ROS_INFO("End effector reference frame: %s", arm.getEndEffectorLink().c_str());
//获取终端link的名称
std::string end_effector_link=arm.getEndEffectorLink();
//设置目标位置所使用的参考坐标系
std::string pose_reference_frame="base_link";
arm.setPoseReferenceFrame(pose_reference_frame);
//当运动规划失败后,允许重新规划
arm.allowReplanning(true);
//设置位置(单位:米)和姿态(单位:弧度)的允许误差
arm.setGoalPositionTolerance( 0.1);
arm.setGoalOrientationTolerance( 0.1);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
//控制机械臂运动到之前设置的“forward”姿态
arm.setNamedTarget("forward");
arm.move();
//获取当前位姿数据最为机械臂运动的起始位姿
geometry_msgs::Pose start_pose=arm.getCurrentPose (end_effector_link).pose;
std::vector<geometry_msgs::Pose > waypoints;
//waypoints.push_back(start_pose);
geometry_msgs::Pose wpose=start_pose;
wpose.position.x -= 0.2;
wpose.position.y -= 0.2;
waypoints.push_back(wpose);
wpose.position.x+=0.05;
wpose.position.y+=0.15;
wpose.position.z-=0.15;
waypoints.push_back(wpose);
waypoints.push_back(start_pose);
if(cartesian)
{
double fraction = 0.0 ;// #路径规划覆盖率
int maxtries = 100 ;// #最大尝试规划次数
int attempts = 0;// #已经尝试规划次数
//# 设置机器臂当前的状态作为运动初始状态
arm.setStartStateToCurrentState();
moveit_msgs::RobotTrajectory trajectory;
//# 尝试规划一条笛卡尔空间下的路径,依次通过所有路点
while (fraction < 1.0 && attempts < maxtries)
{
//返回一个值,该值介于0.0到1.0之间,表示路径点所描述的实现路径的百分比。如果出错,返回-1.0。
fraction= arm.computeCartesianPath(
waypoints, //# waypoint poses,路点列表
0.01, //# eef_step,终端步进值
0.0, // # jump_threshold,跳跃阈值
trajectory,
true,NULL); // # avoid_collisions,避障规划
// # 尝试次数累加
attempts += 1;
//# 打印运动规划进程
if (attempts % 10 == 0)
ROS_INFO("still trying after %d attempts...",attempts);
}
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
my_plan.trajectory_=trajectory;
//按照规划的运动路径控制机械臂运动
arm.execute(my_plan);
// # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
if (fraction == 1.0)
{
ROS_INFO("Path computed successfully. Moving the arm %f.",fraction);
arm.move();
sleep(5);
ROS_INFO("Path execution complete.");
}
//# 如果路径规划失败,则打印失败信息
else
ROS_INFO("Path planning failed with only %f success after %d attempts",fraction,maxtries);
}
/////////////////////////////////////////////////////////////////////
//控制机械臂先回到初始化位置
arm.setNamedTarget("home");
arm.move();
sleep(2);
ros::shutdown();
}