MoveIt编程实现笛卡尔空间机械臂运动

前两篇文章是关于在关节空间中进行机械臂的运动控制：

MoveIt简单编程实现关节空间机械臂运动(逆运动学)

MoveIt简单编程实现关节空间机械臂运动(正运动学)

通过对关节空间下的机器人6个轴进行控制，每个轴的变化都是通过插补进行完成运动，六个轴互相不会关心其他轴是如何运动的，机器人轴端在两个点之间走出的轨迹是任意的曲线。

但是对于一些任务重，要求机械臂终端轨迹的形状是直线或者圆弧等，即对轨迹的形状是有要求的，这个时候就要使用笛卡尔孔家，增加笛卡尔路径约束。

首先使用以下命令，看一下执行的效果：

roslaunch probot_anno_moveit_config demo.launch

走直线：

rosrun probot_demo moveit_cartesian_demo.py _cartesian:=True

自由曲线：

rosrun probot_demo moveit_cartesian_demo.py _cartesian:=False

效果：

为了更好的看一下机械臂末端的运动轨迹，将轨迹显示出来如图所以：

Python代码以及解析：

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopyclass MoveItCartesianDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_cartesian_demo', anonymous=True)# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线cartesian = rospy.get_param('~cartesian', True)# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander('manipulator')# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置目标位置所使用的参考坐标系arm.set_pose_reference_frame('base_link')# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.001)arm.set_goal_orientation_tolerance(0.001)# 设置允许的最大速度和加速度arm.set_max_acceleration_scaling_factor(0.5)arm.set_max_velocity_scaling_factor(0.5)# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 获取当前位姿数据最为机械臂运动的起始位姿start_pose = arm.get_current_pose(end_effector_link).pose# 初始化路点列表waypoints = []# 如果为True,将初始位姿加入路点列表if cartesian:waypoints.append(start_pose)# 设置路点数据，并加入路点列表，所有的点都加入wpose = deepcopy(start_pose)#拷贝对象wpose.position.z -= 0.2if cartesian:  #如果设置为True，那么走直线waypoints.append(deepcopy(wpose))else:          #否则就走曲线arm.set_pose_target(wpose)  #自由曲线arm.go()rospy.sleep(1)wpose.position.x += 0.15if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.y += 0.1if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.x -= 0.15wpose.position.y -= 0.1if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)#规划过程if cartesian:fraction = 0.0   #路径规划覆盖率maxtries = 100   #最大尝试规划次数attempts = 0     #已经尝试规划次数# 设置机器臂当前的状态作为运动初始状态arm.set_start_state_to_current_state()# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点while fraction < 1.0 and attempts < maxtries:#规划路径 ，fraction返回1代表规划成功(plan, fraction) = arm.compute_cartesian_path (waypoints,   # waypoint poses，路点列表，这里是5个点0.01,        # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达0.0,         # jump_threshold，跳跃阈值，设置为0代表不允许跳跃True)        # avoid_collisions，避障规划# 尝试次数累加attempts += 1# 打印运动规划进程if attempts % 10 == 0:rospy.loginfo("Still trying after " + str(attempts) + " attempts...")# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动if fraction == 1.0:rospy.loginfo("Path computed successfully. Moving the arm.")arm.execute(plan)rospy.loginfo("Path execution complete.")# 如果路径规划失败，则打印失败信息else:rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  rospy.sleep(1)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)if __name__ == "__main__":try:MoveItCartesianDemo()except rospy.ROSInterruptException:pass

获取参数，使用 rospy.get_param(param_name)：

# 获取全局参数
rospy.get_param('/global_param_name')# 获取目前命名空间的参数
rospy.get_param('param_name')# 获取私有命名空间参数
rospy.get_param('~private_param_name')# 获取参数，如果没，使用默认值
rospy.get_param('foo', 'default_value')

2.路径规划API，(plan, fraction) = arm.compute_cartesian_path

返回值：

plan：规划出来的运动轨迹
fraction：描述规划成功的轨迹在给定路径点列表的覆盖率【0~1】。如果fraction小于1，说明给定的路径点列表没有办法完整规划

C++代码解析：

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>int main(int argc, char **argv)
{//初始化节点ros::init(argc, argv, "moveit_cartesian_demo");//引入多线程ros::AsyncSpinner spinner(1);//开启多线程spinner.start();//初始化需要使用move group控制的机械臂中的arm groupmoveit::planning_interface::MoveGroupInterface arm("manipulator");//获取终端link的名称std::string end_effector_link = arm.getEndEffectorLink();//设置目标位置所使用的参考坐标系std::string reference_frame = "base_link";arm.setPoseReferenceFrame(reference_frame);//当运动规划失败后，允许重新规划arm.allowReplanning(true);//设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.setGoalPositionTolerance(0.001);arm.setGoalOrientationTolerance(0.01);//设置允许的最大速度和加速度arm.setMaxAccelerationScalingFactor(0.5);arm.setMaxVelocityScalingFactor(0.5);// 控制机械臂先回到初始化位置arm.setNamedTarget("home");arm.move(); //规划+移动sleep(1);  //停1s// 获取当前位姿数据最为机械臂运动的起始位姿geometry_msgs::Pose start_pose = arm.getCurrentPose(end_effector_link).pose;//初始化路径点向量std::vector<geometry_msgs::Pose> waypoints;//将初始位姿加入路点列表waypoints.push_back(start_pose);start_pose.position.z -= 0.2;waypoints.push_back(start_pose);start_pose.position.x += 0.1;waypoints.push_back(start_pose);start_pose.position.y += 0.1;waypoints.push_back(start_pose);// 笛卡尔空间下的路径规划moveit_msgs::RobotTrajectory trajectory;const double jump_threshold = 0.0;const double eef_step = 0.01;double fraction = 0.0;int maxtries = 100;   //最大尝试规划次数int attempts = 0;     //已经尝试规划次数while(fraction < 1.0 && attempts < maxtries){fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);attempts++;if(attempts % 10 == 0)ROS_INFO("Still trying after %d attempts...", attempts);}if(fraction == 1){   ROS_INFO("Path computed successfully. Moving the arm.");// 生成机械臂的运动规划数据moveit::planning_interface::MoveGroupInterface::Plan plan;plan.trajectory_ = trajectory;// 执行运动arm.execute(plan);sleep(1);}else{ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);}// 控制机械臂先回到初始化位置arm.setNamedTarget("home");arm.move();sleep(1);ros::shutdown(); return 0;
}

MoveIt编程实现笛卡尔空间机械臂运动相关推荐

驱动器空间、关节空间与笛卡尔空间
驱动器空间.关节空间与笛卡尔空间一般来说,操作臂的位置和姿态描述有三种表示方法,分别为驱动器空间描述.关节空间描述和笛卡尔空间描述.三者之间有如下的映射关系: 在进行机械臂运动学分析时,我们必须弄清 ...
笛卡尔空间轨迹规划（直线、圆弧）
目录毕设(5)-笛卡尔空间轨迹规划(直线.圆弧) 直线轨迹规划圆弧轨迹规划 Matlab代码验证毕设中用到了很多代码,其中一部分我通过看书和看论文学习并实现的代码,会通过Gitee仓库分享出来, ...
「运动控制」“关节空间与笛卡尔空间进行轨迹规划”研究
1.什么是轨迹规划? 轨迹规划是指根据作业任务的要求,确定轨迹参数并实时计算和生成运动轨迹. 2.轨迹规划目的它是机器人研究领域中的一个很重要的内容,是工业机器人控制的依据,所有控制的目的都是要围绕 ...
笛卡尔空间直线轨迹规划——S型曲线加减速插补算法（含MATLAB仿真验证代码）
写这个算法,是因为博主正在做一个机械臂和全向小车的项目,里面涉及需要笛卡尔空间做直线轨迹的规划.通常的算法有梯型加减速,这个算法只是速度连续,加速度并不连续.所以实际冲击较大,无法实现柔性控制.况且网 ...
python控制机械臂6轴_在ROS环境下，怎么使用moveit!来驱动真实的六轴机械臂？
很多小伙伴在使用ROS的时候,都会产生类似的疑问,程序写过那么多,仿真也跑过不少,但是如何控制真实机械臂/机器人呢? 今天古月君就来尝试破个题. 首先,解决这个问题的关键词是"接口" ...
MoveIt的使用（二）机械臂URDF在ROS中MoveIt的配置和使用
一.机械臂在ros里面的可视化在SOLIDWORKS转换成urdf文件之后,生成的文件放在新建的功能包中后,编译(catkin_make),再执行下面代码即可运行可视化文件 roslaunch de ...
（学习用1）调用用RRT算法进行笛卡尔空间轨迹规划和关节空间轨迹规划
在MoveIt中,可以通过调用computeCartesianPath()函数来使用RRT算法进行笛卡尔空间轨迹规划,可以通过调用computeJointSpacePath()函数来使用RRT算法进行 ...
机器人轨迹规划：简单的笛卡尔空间/关节空间轨迹规划方案
文章目录知乎:[OpenRobotSL](https://www.zhihu.com/people/OpenRobotSL) 写在前面 1.关节空间同步运动 2.如何处理运动过程中的平滑性问题 3. ...
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂（双臂）轨迹跟踪控制matlab仿真程序
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序,含空间机器人动力学模型,PD控制程序,带仿真结果,可供二次开发学习 ID:672006146 ...

MoveIt编程实现笛卡尔空间机械臂运动

MoveIt编程实现笛卡尔空间机械臂运动相关推荐

最新文章

热门文章