问题描述:
请看代码,pos存储三维点,posOffset存储三维点的偏差。
想通过设置一个初始点,然后根据偏差值规划一系列的点,期望机械臂末端(法兰盘中心)根据这些点运动。
目前遇到的问题:假设规划路径为A---B---C---……,A---B时,机器人会绕一整圈到达B点,并没有实现直线到达。
请哪位大神帮忙分析,万分谢谢!
//实现关节空间轨迹规划 #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 <vector>
int main(int argc, char **argv) { //初始化,其中ur_test02为节点名 ros::init(argc, argv, "ur_test01"); //多线程 ros::AsyncSpinner spinner(1); //开启新的线程 spinner.start(); //初始化需要使用move group控制的机械臂中的arm group moveit::planning_interface::MoveGroupInterface arm("manipulator"); //允许误差 arm.setGoalJointTolerance(0.001); //允许的最大速度和加速度 arm.setMaxAccelerationScalingFactor(0.2); arm.setMaxVelocityScalingFactor(0.2);
// 控制机械臂先回到水平位置,这个位置下载的文件中已经设置好了 arm.setNamedTarget("home"); arm.move(); sleep(1); //控制机械臂到达竖直位置,这个位置下载的文件中也设置好了 // arm.setNamedTarget("up"); // arm.move(); //sleep(1);
//关节空间 std::vector<vector<double>> pos = { { 1516.38824 24.37429249 4.945091105 }, { 1517.288232 24.70266148 4.910326798 }, { 1517.828202 25.1301746 4.869095553 }, { 1518.6382 25.65316643 4.823924466 }, { 1519.538144 26.23986586 4.779723388 }, { 1520.43816 26.84294131 4.7401259 }, { 1521.518101 27.43266236 4.705291704 }, { 1522.418002 27.97653605 4.675852832 }, { 1523.317976 28.47903696 4.648267775 }, { 1524.038019 28.94768219 4.620115082 }, { 1524.847979 29.38823305 4.590313158 } }; int n = pos.size(); int m = pos[0].size(); std::vector<vector<double>> posOffset(n-1,vector<double>(m,0)); for(int i = 0; i < n-1; i++){ posOffset[i][1] = pos[i+1][1] - pos[i][1]; posOffset[i][2] = pos[i+1][2] - pos[i][2]; posOffset[i][3] = pos[i+1][3] - pos[i][3]; }
geometry_msgs::Pose target_pose1; target_pose1.orientation.w = 1.0; target_pose1.position.x = 0.35; target_pose1.position.y = -0.05; target_pose1.position.z = 0.6; arm.setPoseTarget(target_pose1);
for(int i = 0; i < n-1; i++){ target_pose1.position.x += posOffset[i][1]; target_pose1.position.y += posOffset[i][2]; target_pose1.position.z += posOffset[i][3]; arm.setPoseTarget(target_pose1); }
arm.move(); //输出确定是否运行成功
moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); //控制机械臂先回到初始化位置 //arm.setNamedTarget("home"); //arm.move(); // sleep(1); //ros::shutdown();
return 0; } |
|
第三方账号登入
QQ 微博 微信