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
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
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)
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_fab.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)