第七章作业
本次作业用的是Marm机械臂。过程如下：
第一题 使用自己的机械臂模型，分别编写程序，实现以下功能：
（1）圆弧运动：机械臂终端完成圆弧轨迹的规划运动，半径和圆心根据模型确定接口；
（2）轨迹重定义：针对规划得到的轨迹，缩减1/4的轨迹点，并完成运动；例如：原来有20个轨迹点，每隔4个删掉一个，最后得到16个点（首尾两点不能删除），再完成运动；
（3）多轨迹连续运动：完成至少两条轨迹的拼接、重规划和连续运动，具体运动类型不限制。
1.1 第一小问：圆弧运动
MoveIt!可以规划笛卡尔直线运动，那能不能规划圆弧运动，甚至任意曲线轨迹的运动呢？MoveIt!中没有直接的API接口，但是通过直线逼近的方式，就可以实现。如图1-1所示。

图1-1 直线逼近实现圆周运动
在终端下输入roslaunch marm_moveit_config demo.launch与rosrun marm_demo moveit_circle_demo.py，效果如图1-2所示。

图1-2 圆弧运动
该例程实现的流程：首先通过圆的轨迹方程，计算得到一系列圆上的路径点，然后使用computeCartesianPath函数完成所有路径点之间的直线轨迹规划，最后execute执行运动。
程序步骤：
a) 创建规划组的控制对象；
b) 获取机器人的终端link全称；
c) 设置目标位姿对应的参考坐标系和起始位姿；
d) 计算圆弧轨迹并规划路径；
e) 完成规划并控制机械臂完成运动。
关键程序如下：
a）设置机械臂开始画圆的初始位置，即圆心位置

target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now() 
target_pose.pose.position.x = 0.37291
target_pose.pose.position.y = 0.0046782
target_pose.pose.position.z = 0.36041
target_pose.pose.orientation.x = 0.99493
target_pose.pose.orientation.y = 0.039681
target_pose.pose.orientation.z = 0.0030147
target_pose.pose.orientation.w = -0.092408

b）计算圆弧轨迹

for th in numpy.arange(0, 6.28, 0.04):
target_pose.pose.position.y = centerA + radius * math.cos(th)
target_pose.pose.position.z = centerB + radius * math.sin(th)
wpose = deepcopy(target_pose.pose)
waypoints.append(deepcopy(wpose))

c）规划笛卡尔路径

while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划

1.2 第二小问：轨迹重定义
很多情况下，我们对MoveIt!规划的轨迹并不满意，那可以通过学习moveit编程的API文档对这条轨迹进行修改。在终端下输入：

roslaunch marm_moveit_config demo.launch
rosrun marm_demo moveit_revise_trajectory_demo 

运行成功后，我们可以看到机器人会完成两次类似的运动，但是路径长度有了差异，这就是因为我们对MoveIt!规划的轨迹做了人为的修改。如图1-3所示。

图1-3 轨迹重定义
该例程实现的流程：首先使用plan正常完成规划，然后遍历规划得到的轨迹数据，里边的位置、速度、加速度、时间想怎么改就怎么改了，最后execute执行修改之后的轨迹即可。
程序步骤：
a）创建规划组的控制对象；
b）设置起始位姿与目标位姿；
c）获取moveit的plan轨迹；
d）针对plan这个数据结构的数据进行修改，如修改速度、时间、位置等；
e）完成规划并控制机械臂完成运动。
关键程序如下
a）轨迹规划：

# Get back the planned trajectory
arm.set_joint_value_target(joint_positions)
traj = arm.plan()

b）轨迹重定义：

m=0
points.pop()
k = n_points//4
while(m<k):
m += 4
del points[m]
for i in range(n_points-k):
point = JointTrajectoryPoint()
# The joint positions are not scaled so pull them out first
point.positions = traj.joint_trajectory.points[i].positions
# Store the scaled trajectory point
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
return new_traj

1.3 第三小问：多轨迹连续运动
到现在为止，例程中机械臂每次运动的速度变化都是从0到0的，在连续运动中会出现频繁的停顿，下面介绍一种将多个轨迹链接到一起的方法。在终端输入：

roslaunch marm_moveit_config demo.launch
rosrun marm_demo moveit_continue_trajectory_demo

运行多轨迹连续运动的例程，机械臂会一次完成两条轨迹的运动，中间并不会出现停顿的问题。效果如图1-4所示。

图1-4 多轨迹连续运行
实现这个例程用到了很多MoveIt!深层的API。主要流程：首先规划多条需要连续运动的轨迹，但是此时机器人并没有真实运动，所以需要注意后续轨迹规划的起点应该是前一条轨迹的终点。
程序步骤：
a）创建规划组的控制对象；
b）设置多条轨迹起始位姿与目标位姿；
c）获取moveit的plan轨迹；
d）连接两条轨迹；
e）完成规划并控制机械臂完成运动。
主要程序：
a）先设置目标点以及利用moveit获取运动轨迹。

//设置第一个目标点
joint_group_positions[2] = -1.2; // radians
arm.setJointValueTarget(joint_group_positions);
// 计算第一条轨迹
moveit::planning_interface::MoveGroupInterface::Plan plan1;
moveit::planning_interface::MoveItErrorCode success = arm.plan(plan1);
joint_model_group = start_state->getJointModelGroup(arm.getName()); 
start_state->setJointGroupPositions(joint_model_group, joint_group_positions);
arm.setStartState(*start_state);
//设置第二个目标点
joint_group_positions[3] = -1.2; // radians
joint_group_positions[4] = -1.5; // radians
arm.setJointValueTarget(joint_group_positions);
// 计算第二条轨迹
moveit::planning_interface::MoveGroupInterface::Plan plan2;
success = arm.plan(plan2);

b）然后创建一个新的轨迹对象，把以上规划好的轨迹全部push进去，再调用IPTP时间最优算法的接口，重新规划这条新轨迹的速度、加速度、时间等信息，然后执行。

//连接两条轨迹
moveit_msgs::RobotTrajectory trajectory;
trajectory.joint_trajectory.joint_names = plan1.trajectory_.joint_trajectory.joint_names;
trajectory.joint_trajectory.points = plan1.trajectory_.joint_trajectory.points;
for (size_t j = 1; j < plan2.trajectory_.joint_trajectory.points.size(); j++)
{
trajectory.joint_trajectory.points.push_back(plan2.trajectory_.joint_trajectory.points[j]);
}
//重规划
moveit::planning_interface::MoveGroupInterface::Plan joinedPlan;
robot_trajectory::RobotTrajectory rt(arm.getCurrentState()->getRobotModel(), "arm");
rt.setRobotTrajectoryMsg(*arm.getCurrentState(), trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;

第二题 修改运动学插件，测试运行以上例程
TRAC-IK的使用方法很简单，安装后修改配置文件就可以启用了。安装命令：sudo apt-get install ros-melodic-trac-ik-kinematics-plugin，接着在marm_moveit_config/config文件下面的kinematics.yaml文件修改成：

arm:
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
kinematics_solver_search_resolution: 0.005

然后再运行上面例程的命令就可以了。