3.5. Plan motion
Note
The following examples use the ROS backend and the MoveI! planner for UR5 robots. Before running them, please make sure you have the ROS backend correctly configured and the UR5 Demo started.
There are 2 function that allow to plan a robotic movement without collisions:
plan_cartesian_motion
and
plan_motion
.
3.5.1. Plan cartesian motion
import time
from compas.datastructures import Mesh
from compas.geometry import Frame
from compas.robots import Configuration
import compas_fab
from compas_fab.backends import RosClient
from compas_fab.robots import CollisionMesh, AttachedCollisionMesh
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
ee_link_name = robot.get_end_effector_link_name()
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
cm = CollisionMesh(mesh, 'tip')
acm = AttachedCollisionMesh(cm, link_name=ee_link_name)
client.add_attached_collision_mesh(acm)
frames = []
frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000])
options = {
'max_step': 0.01,
'avoid_collisions': True,
}
trajectory = robot.plan_cartesian_motion(frames,
start_configuration,
options=options)
print("Computed cartesian path with %d configurations, " % len(trajectory.points))
print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
print("Path plan computed with the attached collision meshes: {}".format(
[acm.collision_mesh.id for acm in trajectory.attached_collision_meshes]))
3.5.2. Plan motion
In contrast to the cartesian path, the plan_motion
allows to describe the goal with constraints rather than defined frames.
import math
from compas.geometry import Frame
from compas.robots import Configuration
from compas_fab.backends import RosClient
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.])
group = robot.main_group_name
# create goal constraints from frame
goal_constraints = robot.constraints_from_frame(frame,
tolerance_position,
tolerance_axes,
group)
trajectory = robot.plan_motion(goal_constraints,
start_configuration,
group,
options=dict(
planner_id='RRTstarkConfigDefault'
))
print("Computed kinematic path with %d configurations." % len(trajectory.points))
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)