Plan motion

Note

The following examples use the ROS backend and the MoveIt! 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.

Plan cartesian motion

Cartesian planning allows to plan motions that follow a sequence of defined frames.

from compas.geometry import Frame

from compas_fab.backends import RosClient
from compas_fab.robots import FrameWaypoints

with RosClient() as client:
    robot = client.load_robot()
    assert robot.name == "ur5_robot"

    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]))
    waypoints = FrameWaypoints(frames)

    start_configuration = robot.zero_configuration()
    start_configuration.joint_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(waypoints, 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)

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.backends import RosClient
from compas_fab.robots import FrameTarget

with RosClient() as client:
    robot = client.load_robot()
    assert robot.name == 'ur5_robot'

    frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
    tolerance_position = 0.001
    tolerance_orientation = math.radians(1)

    start_configuration = robot.zero_configuration()
    start_configuration.joint_values = (-3.530, 3.830, -0.580, -3.330, 4.760, 0.000)
    group = robot.main_group_name

    # create target from frame
    target = FrameTarget(frame,
                         tolerance_position,
                         tolerance_orientation,
                         )

    trajectory = robot.plan_motion(target,
                                   start_configuration,
                                   group,
                                   options=dict(
                                       planner_id='RRTConnect'
                                   ))

    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)