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

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]))

    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(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)

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

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_axes = [math.radians(1)] * 3

    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 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='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)