Z's learning note

Search

SearchSearch

moveit

Jan 24, 2022, 14 min read

Moveit

问题

  • 带约束的运动规划中，可不可以强制指定某个关节的值，完成剩下关节的规划？还是必须修改MoveGroup? 强制指定最后一个关节时的解决方案：自行编写运动规划代码，逐点求IK然后插值，倒数第二个连杆的末端姿态可以用DH倒推

    You can setup JointModelGroups (planning groups) for each single joint in your srdf to plan with these joints only.

  • 如何配置MoveGroup？是否可以配置多个MoveGroup（关节的使用上有重复）

  • 是否可以在机器人实际移动之前获取规划的关节运动轨迹（关节数值）？

  • 之前的代码好像只是添加了物体，但是不是碰撞对象

    planning_scene_interface.addCollisionObjects(collision_objects);

准备工作

覆盖机器人默认的关节限位设置

  • 若需要自定义机器人关节限位以实现特定目的：
  • 参考
  • 通过rosparam修改关节上下限（若直接调用IK求解器的API，将不会起作用）rospy.set_param('/robot_description_planning/joint_limits/yumi_joint_1_l/max_position', 0)

运动规划求解器

机械臂的运动控制

综述，ROS下运动规划的方法

  • 几种运动规划方法的异同

    | 方法 | 大类 | 输入输出 | 局部最优的处置 | 耗时 | 调参（parameter tuning） | 避障 | | ------- | ---- | -------------- | ---- | ------------------------ | ---- | | 手动规划 | | | | | | | OMPL | | | | | | | CHOMP | | | | | | | STOMP | | | 通过统计（stochastic）方式避免陷入局部最优 | | | | TrajOpt | | | | | | | Pilz | | | | | |

OMPL

  • Open Motion Planning Library
  • sampling based / randomized motion planning algorithms

CHOMP

  • Covariant Hamiltonian optimization for motion planning
  • 基于梯度方法的路径最优化gradient-based trajectory optimization

STOMP

  • Stochastic Trajectory Optimization for Motion Planning
  • 基于概率方法的路径最优化probabilistic optimization framework

TrajOpt

Pilz Industrial Motion Planner

逆向运动学的求解

  • 任务：给定Task space中的一点作为末端的位姿，依据机器人的关节配置，反向求得各个关节应处于的角度/位移
  • 三种主流求解器：KDL，TRAC-IK，IKFast
    • Orocos-KDL：默认求解器，数值解，效率较低
    • TRAC-IK：数值解，效率较高 TRAC-IK求解器以机械臂的link（可以是其中的一段）作为实例化参数。求解时输入为初始状态，输出为目标位姿时各个关节的控制量。
    • IKFast：解析解
  • KDL为默认求解器，如使用另外两种求解器需要另行配置
  • 其它IK求解器：RelaxedIK系列
逆向运动学的配置文件
  • kinematics.yaml
    • kinematics_solver：所使用的求解器
    • kinematics_solver_search_resolution
    • kinematics_solver_timeout
    • kinematics_solver_attempts
    • position_only_ik：可选，求解时只需要位置信息不需要方向信息。position_only_ik: True

正向运动学service

rospy.wait_for_service('compute_fk')  
try:  
  moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)  
except rospy.ServiceException, e:  
  rospy.logerror("Service call failed: %s"%e)

逆向运动学service

使用Moveit进行运动规划的步骤（API）

参考

准备工作：

控制流程：初始化机器人、场景

moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
  • group names 在初始化机器人后，可以通过group_names=robot.get_group_names()来获取机器人所有的控制组MoveGroup，再将其中的group应用于实例化控制组group = moveit_commander.MoveGroupCommander(group_name)
  • 机器人当前状态 robot.get_current_state()

初始化控制组MoveGroupCommander

left_group = moveit_commander.MoveGroupCommander('left_arm')
从控制组中读取信息：
  • planning frame planning_frame = left_group.get_planning_frame()
  • name of the end-effector link eef_link = left_group.get_end_effector_link()
  • 控制组中所有关节的当前角度值 left_group.get_current_joint_values()

两种控制方式：作业空间和关节空间的运动控制

作业空间运动控制Workspace/Cartesian space

确定关键路径点waypoints
  • waypoints为一个list，其中每个元素均为Pose()，设置其position的x，y，z和orientation的x，y，z，w。
针对关键路径点完成路径规划：
  • 确定关键点后交由compute_cartesian_path规划具体路径
  • group为之前初始化的控制组
# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation.  We will disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
                                waypoints,   # waypoints to follow
                                0.01,        # eef_step
                                0.0)         # jump_threshold
  • fraction:
    • fraction小于1时说明路径规划失败
显示规划路径

Displaying a Trajectory You can ask RViz to visualize a plan (aka trajectory) for you. But the group.plan() method does this automatically so this is not that useful here (it just displays the same trajectory again):

A DisplayTrajectory msg has two primary fields, trajectory_start and trajectory. We populate the trajectory_start with our current robot state to copy over any AttachedCollisionObjects and add our plan to the trajectory.

display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan)

Publish

display_trajectory_publisher.publish(display_trajectory);

import moveit_msgs.msg
 
display_trajectory_publisher = rospy.Publisher(
    "/move_group/display_planned_path",
    moveit_msgs.msg.DisplayTrajectory,
    queue_size=20,
)
 
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory);
执行（需指定执行路径的group）：
`left_group.execute(plan, wait=True)`
其它：
所提供的路径关键点与规划后执行的轨迹不一致？：
无法设置最大运行速度
  • Cartesian space的运动规划不支持maxVelocityScalingFactor参数

    Cartesian motions should often be slow, e.g. when approaching objects. The speed of Cartesian plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time the trajectory manually, as described here. Pull requests are welcome.

双臂的运动规划
  • 通过Actionlib实现同时控制双臂进行运动 参考Yumi的示例

关节空间运动控制 Joint space

  • 读取当前关节角度get.current_joint_values()
group.get_current_joint_values()
  • 设置关节角度的几种方式
    • 一次性设置关节运动目标角度set_joint_value_target()
    group.set_joint_value_target(value_list)
    • 使用底层关节运动轨迹控制器
底层关节运动轨迹控制器 Joint Trajectory Action
概览
  • YAML的配置
    • action_ns：ns —— namespace 例如FollowJointTrajectory action
actionlib
通过消息进行控制
通过actionlib的client进行关节控制
  • 相比通过消息进行控制：
    1. 需要创建客户端 client = actionlib.SimpleActionClient('/robot/joint_traj_pos_controller/follow_joint_trajectory', FollowJointTrajectoryAction) 服务名可通过查看MoveIt配置文件，或者rostopic列表获得，类型为FollowJointTrajectoryAction
    2. 等待客户端连接倒服务 client.wait_for_server()
    3. 构建关节路径（注意joint_names必须包含运动组中的全部关节）
     goal =  FollowJointTrajectoryGoal()
 goal.trajectory.joint_names = ['joint_1','joint_2','joint_3','joint_4','joint_5','joint_6']
 for i in range(len(traj)):
     point = JointTrajectoryPoint()
     point.positions = traj[i]
     point.time_from_start = rospy.Duration(10+i)
 
     goal.trajectory.points.append(point)
    1. 发送目标路径 client.send_goal(goal)
    2. 等待路径执行结束 client.wait_for_result()
    3. 获取结果 client.get_result()
  • 实例使用actionlib控制Yumi

向场景中添加物体

准备工作：获取场景

moveit_commander.roscpp_initialize(sys.argv)
scene = moveit_commander.PlanningSceneInterface()

添加一个障碍物

  1. 添加物体 
    box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "world"
box_pose.pose.orientation.w = 1.0
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
  2. 更新碰撞检测 
    start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
  # Test if the box is in the scene.
  # Note that attaching the box will remove it from known_objects
  is_known = box_name in scene.get_known_object_names()
 
  # Test if we are in the expected state
  if (box_is_known == is_known):
    return True
 
  # Sleep so that we give other threads time on the processor
  rospy.sleep(0.1)
  seconds = rospy.get_time()
 
# If we exited the while loop without returning then we timed out
return False
  3. 如果物体的名字相同，使用scene.add_box添加新的物体将覆盖上一次添加的物体（更新物体位置姿态信息不需要删除？）
  4. 如果添加后没有在场景内显示出来，可能是添加失败。可能的失败原因是node初始化后需要延时（或者延时时间不够）

官方示例：pick-n-place任务。共分五个步骤

  1. 添加物体

Adding Objects to the Planning Scene First, we will create a box in the planning scene at the location of the left finger:

box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
  1. 更新碰撞检测

Ensuring Collision Updates Are Receieved If the Python node dies before publishing a collision object update message, the message could get lost and the box will not appear. To ensure that the updates are made, we wait until we see the changes reflected in the get_known_object_names() and get_known_object_names() lists. For the purpose of this tutorial, we call this function after adding, removing, attaching or detaching an object in the planning scene. We then wait until the updates have been made or timeout seconds have passed

start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
  # Test if the box is in attached objects
  attached_objects = scene.get_attached_objects([box_name])
  is_attached = len(attached_objects.keys()) > 0
 
  # Test if the box is in the scene.
  # Note that attaching the box will remove it from known_objects
  is_known = box_name in scene.get_known_object_names()
 
  # Test if we are in the expected state
  if (box_is_attached == is_attached) and (box_is_known == is_known):
    return True
 
  # Sleep so that we give other threads time on the processor
  rospy.sleep(0.1)
  seconds = rospy.get_time()
 
# If we exited the while loop without returning then we timed out
return False
  1. 拾取后将物体附加在机器人上

Next, we will attach the box to the Panda wrist. Manipulating objects requires the robot be able to touch them without the planning scene reporting the contact as a collision. By adding link names to the touch_links array, we are telling the planning scene to ignore collisions between those links and the box. For the Panda robot, we set grasping_group = ‘hand’. If you are using a different robot, you should change this value to the name of your end effector group name.

grasping_group = 'hand'
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)
MoveGroupCommander.attach_object
(_self_,
_object_name_,
_link_name_ = "",
_touch_links_ = [] )
  1. 放下物体时断开物体和机器人的连接

We can also detach and remove the object from the planning scene:

scene.remove_attached_object(eef_link, name=box_name)
  1. 从规划场景中移除物体

We can remove the box from the world.

scene.remove_world_object(box_name)

碰撞检测

  • test

  • If we want to test collision in a particular group, we can do that by mentioning group_name, as shown in the following code. Here, the group_name is arm:

collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix(); 
robot_state::RobotState copied_state = planning_scene.getCurrentState(); 
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); 
ROS_INFO_STREAM("6. Full collision Test: "<< (collision_result.collision ? "in" : "not in") << " collision");

传感器数据的引入

Planning scene规划场景的API

参考

待整理的参考资料

自定义关节限位

setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it.

/* Set one joint in the Panda arm outside its joint limit */
joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
 
/* Check whether any joint is outside its joint limits */
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
 
/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
  • 自定配置文件覆盖默认配置？
  • In normal MoveIt configuration packages the planning_context.launch file will also load a joint_limits.yaml file from the config directory which allows you / your users to override those limits (here).

So if has_position_limits is true in that file, the position limit from the urdf will not be used (or: will be overridden by the value in that .yaml file).

Backlinks