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.

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

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)