Motion Designator

Motion designators are similar to action designators, but unlike action designators, motion designators represent atomic low-level motions. Motion designators only take the parameter that they should execute and not a list of possible parameters, like the other designators. Like action designators, motion designators can be performed, performing motion designator verifies the parameter and passes the designator to the respective process module.

Since motion designators perform a motion on the robot, we need a robot which we can use. Therefore, we will create a BulletWorld as well as a PR2 robot.

[1]:

from pycram.bullet_world import BulletWorld, Object
from pycram.enums import ObjectType

world = BulletWorld()
pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")

pybullet build time: May 20 2022 19:44:17
Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']
[WARN] [1706546816.907019]: Failed to import Giskard messages
[WARN] [1706546816.911886]: Could not import RoboKudo messages, RoboKudo interface could not be initialized
Unknown tag "material" in /robot[@name='plane']/link[@name='planeLink']/collision[1]
Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink']
Unknown tag "material" in /robot[@name='plane']/link[@name='planeLink']/collision[1]
Unknown tag "contact" in /robot[@name='plane']/link[@name='planeLink']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']

The following cell can be used after testing the examples, to close the BulletWorld.

[2]:

world.exit()

Move

Move is used to let the robot drive to the given target pose. Motion designator are used in the same way as the other designator, first create a description then resolve it to the actual designator and lastly, perform the resolved designator.

[3]:

from pycram.pose import Pose
from pycram.designators.motion_designator import MoveMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))

    motion_description.perform()

[4]:

world.reset_bullet_world()

MoveTCP

MoveTCP is used to move the tool center point (TCP) of the given arm to the target position specified by the parameter. Like any designator we start by creating a description and then resolving and performing it.

[5]:

from pycram.designators.motion_designator import MoveTCPMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = MoveTCPMotion(target=Pose([0.5, 0.6, 0.6], [0, 0, 0, 1]), arm="left")

    motion_description.perform()

Looking

Looking motion designator adjusts the robot state such that the cameras point towards the target pose. Although this motion designator takes the target as position and orientation, in reality only the position is used.

[6]:

from pycram.designators.motion_designator import LookingMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = LookingMotion(target=Pose([1, 1, 1], [0, 0, 0, 1]))

    motion_description.perform()

Move Gripper

Move gripper moves the gripper of an arm to one of two states. The states can be open and close, which open and close the gripper respectively.

[7]:

from pycram.designators.motion_designator import MoveGripperMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = MoveGripperMotion(motion="open", gripper="left")

    motion_description.perform()

Detecting

This is the motion designator implementation of detecting, if an object with the given object type is in the field of view (FOV) this motion designator will return an object designator describing the object.

Since we need an object that we can detect, we will spawn a milk for this.

[8]:

milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.5, 0, 1]))

[9]:

from pycram.designators.motion_designator import DetectingMotion, LookingMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).perform()

    motion_description = DetectingMotion(object_type=ObjectType.MILK)

    obj = motion_description.perform()

    print(obj)

ObjectDesignatorDescription.Object(name=milk, type=ObjectType.MILK, bullet_world_object=Object(world=<pycram.bullet_world.BulletWorld object at 0x7ff2c21ae790>,
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7ff29979a580>,
name=milk,
type=ObjectType.MILK,
color=[1, 1, 1, 1],
id=3,
path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf,
joints: ...,
links: ...,
attachments: ...,
cids: ...,
original_pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0,
tf_frame=milk_3,
urdf_object: ...,
_current_pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0,
_current_link_poses: ...,
_current_link_transforms: ...,
_current_joint_states={},
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02],
link_to_geometry: ...), _pose=<bound method Object.get_pose of Object(world=<pycram.bullet_world.BulletWorld object at 0x7ff2c21ae790>,
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7ff29979a580>,
name=milk,
type=ObjectType.MILK,
color=[1, 1, 1, 1],
id=3,
path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf,
joints: ...,
links: ...,
attachments: ...,
cids: ...,
original_pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0,
tf_frame=milk_3,
urdf_object: ...,
_current_pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0,
_current_link_poses: ...,
_current_link_transforms: ...,
_current_joint_states={},
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02],
link_to_geometry: ...)>, pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0)

Move Arm Joints

This motion designator moves one or both arms. Movement targets are a dictionary with joint name as key and target pose as value.

[10]:

from pycram.designators.motion_designator import MoveArmJointsMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = MoveArmJointsMotion(right_arm_poses={"r_shoulder_pan_joint": -0.7})

    motion_description.perform()

World State Detecting

World state detecting is also used to detect objects, however, the object is not required to be in the FOV of the robot. As long as the object is somewhere in the belief state (BulletWorld) a resolved object designator will be returned.

Sine we want to detect something we will spawn an object that we can detect. If you already spawned the milk from the previous example, you can skip this step.

[11]:

milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([-1, 0, 1]))

[12]:

from pycram.designators.motion_designator import WorldStateDetectingMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = WorldStateDetectingMotion(object_type=ObjectType.MILK)

    obj = motion_description.perform()

    print(obj)

Object(world=<pycram.bullet_world.BulletWorld object at 0x7ff2c21ae790>,
local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7ff29979a580>,
name=milk,
type=ObjectType.MILK,
color=[1, 1, 1, 1],
id=3,
path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf,
joints: ...,
links: ...,
attachments: ...,
cids: ...,
original_pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0,
tf_frame=milk_3,
urdf_object: ...,
_current_pose=header:
  seq: 0
  stamp:
    secs: 1706546822
    nsecs: 149801731
  frame_id: "map"
pose:
  position:
    x: 1.5
    y: 0.0
    z: 1.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0,
_current_link_poses: ...,
_current_link_transforms: ...,
_current_joint_states={},
base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02],
link_to_geometry: ...)

Move Joints

Move joints can move any number of joints of the robot, the designator takes two lists as parameter. The first list are the names of all joints that should be moved and the second list are the positions to which the joints should be moved.

[13]:

from pycram.designators.motion_designator import MoveJointsMotion
from pycram.process_module import simulated_robot

with simulated_robot:
    motion_description = MoveJointsMotion(names=["torso_lift_joint", "r_shoulder_pan_joint"], positions=[0.2, -1.2])

    motion_description.perform()