Merge pull request #113 from tthielke/new-process-modules

New process modules

launch/ik_and_description.launch

@@ -29,7 +29,7 @@
   <!-- Donbot -->
   <group if="$(eval robot == 'donbot')">
     <param name="robot_description"
-              textfile="$(find pycram)/resources/donbot.urdf"/>
+              textfile="$(find pycram)/resources/iai_donbot.urdf"/>
   </group>
 
   <!-- HSR -->

resources/boxy.urdf

@@ -3,7 +3,7 @@
 <!-- |    This document was autogenerated by xacro from boxy_description.urdf.xacro    | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
-<robot name="boxy_description" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="boxy" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <material name="DarkGrey">
     <color rgba="0.3 0.3 0.3 1.0"/>
   </material>

src/pycram/bullet_world.py

@@ -274,7 +274,9 @@ def add_vis_axis(self, pose: Pose,
         :param length: Optional parameter to configure the length of the axes
         """
 
-        position, orientation = pose.to_list()
+        pose_in_map = self.local_transformer.transform_pose(pose, "map")
+
+        position, orientation = pose_in_map.to_list()
 
         vis_x = p.createVisualShape(p.GEOM_BOX, halfExtents=[length, 0.01, 0.01],
                                     rgbaColor=[1, 0, 0, 0.8], visualFramePosition=[length, 0.01, 0.01])

src/pycram/process_modules/donbot_process_modules.py

@@ -1,14 +1,19 @@
 import time
 from threading import Lock
 
+import numpy as np
 import pybullet as p
 
 import pycram.bullet_world_reasoning as btr
 import pycram.helper as helper
-from ..bullet_world import BulletWorld
+from ..bullet_world import BulletWorld, Object
+from ..designators.motion_designator import MoveArmJointsMotion, WorldStateDetectingMotion
+from ..external_interfaces.ik import request_ik
 from ..local_transformer import LocalTransformer
+from ..pose import Pose
 from ..process_module import ProcessModule, ProcessModuleManager
 from ..robot_descriptions import robot_description
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
 
 
 def _park_arms(arm):
@@ -30,16 +35,8 @@ class DonbotNavigation(ProcessModule):
     """
 
     def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == 'navigate':
-            robot = BulletWorld.robot
-            # Reset odom joints to zero
-            for joint_name in robot_description.odom_joints:
-                robot.set_joint_state(joint_name, 0.0)
-            # Set actual goal pose
-            robot.set_position_and_orientation(solution['target'], solution['orientation'])
-            time.sleep(0.5)
-            local_transformer.update_from_btr()
+        robot = BulletWorld.robot
+        robot.set_pose(desig.target)
 
 class DonbotPickUp(ProcessModule):
     """
@@ -48,16 +45,18 @@ class DonbotPickUp(ProcessModule):
     """
 
     def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == 'pick':
-            object = solution['object']
-            robot = BulletWorld.robot
-            target = object.get_position()
-            inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(solution['gripper']), target,
-                                               maxNumIterations=100)
-            helper._apply_ik(robot, inv)
-            robot.attach(object, solution['gripper'])
-            time.sleep(0.5)
+        object = desig.object_desig.bullet_world_object
+        robot = BulletWorld.robot
+        grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp)
+        target = object.get_pose()
+        target.orientation.x = grasp[0]
+        target.orientation.y = grasp[1]
+        target.orientation.z = grasp[2]
+        target.orientation.w = grasp[3]
+
+        _move_arm_tcp(target, robot, "left")
+        tool_frame = robot_description.get_tool_frame("left")
+        robot.attach(object, tool_frame)
 
 
 class DonbotPlace(ProcessModule):
@@ -66,56 +65,19 @@ class DonbotPlace(ProcessModule):
     """
 
     def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == 'place':
-            object = solution['object']
-            robot = BulletWorld.robot
-            inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(solution['gripper']), solution['target'],
-                                               maxNumIterations=100)
-            helper._apply_ik(robot, inv)
-            robot.detach(object)
-            time.sleep(0.5)
-
-
-class DonbotAccessing(ProcessModule):
-    """
-    This process module responsible for opening drawers to access the objects inside. This works by firstly moving
-    the end effector to the handle of the drawer. Next, the end effector is moved the respective distance to the back.
-    This provides the illusion the robot would open the drawer by himself.
-    Then the drawer will be opened by setting the joint pose of the drawer joint.
-    """
+        object = desig.object.bullet_world_object
+        robot = BulletWorld.robot
 
-    def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == 'access':
-            kitchen = solution['part_of']
-            robot = BulletWorld.robot
-            gripper = solution['gripper']
-            drawer_handle = solution['drawer_handle']
-            drawer_joint = solution['drawer_joint']
-            dis = solution['distance']
-            inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper),
-                                               kitchen.get_link_position(drawer_handle))
-            helper._apply_ik(robot, inv)
-            time.sleep(0.2)
-            han_pose = kitchen.get_link_position(drawer_handle)
-            new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]]
-            inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), new_p)
-            helper._apply_ik(robot, inv)
-            kitchen.set_joint_state(drawer_joint, 0.3)
-            time.sleep(0.5)
-
-
-class DonbotParkArms(ProcessModule):
-    """
-    This process module is for moving the arms in a parking position.
-    It is currently not used.
-    """
+        # Transformations such that the target position is the position of the object and not the tcp
+        object_pose = object.get_pose()
+        local_tf = LocalTransformer()
+        tcp_to_object = local_tf.transform_pose(object_pose,
+                                                robot.get_link_tf_frame(robot_description.get_tool_frame("left")))
+        target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose()
+
+        _move_arm_tcp(target_diff, robot, "left")
+        robot.detach(object)
 
-    def _execute(self, desig):
-        solutions = desig.reference()
-        if solutions['cmd'] == 'park':
-            _park_arms()
 
 
 class DonbotMoveHead(ProcessModule):
@@ -125,53 +87,40 @@ class DonbotMoveHead(ProcessModule):
     """
 
     def _execute(self, desig):
-        solutions = desig.reference()
-        if solutions['cmd'] == 'looking':
-            robot = BulletWorld.robot
-            neck_base_frame = local_transformer.projection_namespace + '/' + robot_description.chains["neck"].base_link if \
-                local_transformer.projection_namespace else \
-                robot_description.chains["neck"].base_link
-            if type(solutions['target']) is str:
-                target = local_transformer.projection_namespace + '/' + solutions['target'] if \
-                    local_transformer.projection_namespace else \
-                    solutions['target']
-                pose_in_neck_base = local_transformer.tf_transform(neck_base_frame, target)
-            elif helper_deprecated.is_list_pose(solutions['target']) or helper_deprecated.is_list_position(solutions['target']):
-                pose = helper_deprecated.ensure_pose(solutions['target'])
-                pose_in_neck_base = local_transformer.tf_pose_transform(local_transformer.map_frame, neck_base_frame, pose)
-
-            vector = pose_in_neck_base[0]
-            # +x as forward
-            # +y as left
-            # +z as up
-            x = vector[0]
-            y = vector[1]
-            z = vector[2]
-            conf = None
-            if y > 0:
-                conf = "left"
-            else:
-                conf = "right"
-            for joint, state in robot_description.get_static_joint_chain("neck", conf).items():
-                robot.set_joint_state(joint, state)
+        target = desig.target
+        robot = BulletWorld.robot
+
+        local_transformer = LocalTransformer()
+
+        pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link"))
+
+        if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y):
+            robot.set_joint_states(robot_description.get_static_joint_chain("left", "front"))
+        if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x):
+            robot.set_joint_states(robot_description.get_static_joint_chain("left", "arm_right"))
+        if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y):
+            robot.set_joint_states(robot_description.get_static_joint_chain("left", "back"))
+        if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x):
+            robot.set_joint_states(robot_description.get_static_joint_chain("left", "arm_left"))
+
+        pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link"))
+
+        new_pan = np.arctan2(pose_in_shoulder.position.y, pose_in_shoulder.position.x)
+
+        robot.set_joint_state("ur5_shoulder_pan_joint", new_pan + robot.get_joint_state("ur5_shoulder_pan_joint"))
 
 
 class DonbotMoveGripper(ProcessModule):
     """
     This process module controls the gripper of the robot. They can either be opened or closed.
-    Furthermore, it can only moved one gripper at a time.
+    Furthermore, it can only move one gripper at a time.
     """
 
     def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == "move-gripper":
-            robot = BulletWorld.robot
-            gripper = solution['gripper']
-            motion = solution['motion']
-            for joint, state in robot_description.get_static_gripper_chain(gripper, motion).items():
-                # TODO: Test this, add gripper-opening/-closing to the demo.py
-                robot.set_joint_state(joint, state)
-            time.sleep(0.5)
+        robot = BulletWorld.robot
+        gripper = desig.gripper
+        motion = desig.motion
+        robot.set_joint_states(robot_description.get_static_gripper_chain(gripper, motion))
 
 
 class DonbotDetecting(ProcessModule):
@@ -181,17 +130,17 @@ class DonbotDetecting(ProcessModule):
     """
 
     def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == "detecting":
-            robot = BulletWorld.robot
-            object_type = solution['object_type']
-            cam_frame_name = solution['cam_frame']
-            front_facing_axis = solution['front_facing_axis']
+        robot = BulletWorld.robot
+        object_type = desig.object_type
+        # Should be "wide_stereo_optical_frame"
+        cam_frame_name = robot_description.get_camera_frame()
+        # should be [0, 0, 1]
+        front_facing_axis = robot_description.front_facing_axis
 
-            objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type)
-            for obj in objects:
-                if btr.visible(obj, robot.get_link_position_and_orientation(cam_frame_name), front_facing_axis, 0.5):
-                    return obj
+        objects = BulletWorld.current_bullet_world.get_objects_by_type(object_type)
+        for obj in objects:
+            if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis):
+                return obj
 
 
 class DonbotMoveTCP(ProcessModule):
@@ -200,14 +149,10 @@ class DonbotMoveTCP(ProcessModule):
     """
 
     def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == "move-tcp":
-            target = solution['target']
-            gripper = solution['gripper']
-            robot = BulletWorld.robot
-            inv = p.calculateInverseKinematics(robot.id, robot.get_link_id(gripper), target)
-            helper._apply_ik(robot, inv)
-            time.sleep(0.5)
+        target = desig.target
+        robot = BulletWorld.robot
+
+        _move_arm_tcp(target, robot, desig.arm)
 
 
 class DonbotMoveJoints(ProcessModule):
@@ -216,37 +161,34 @@ class DonbotMoveJoints(ProcessModule):
     list that should be applied or a pre-defined position can be used, such as "parking"
     """
 
-    def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == "move-arm-joints":
-            robot = BulletWorld.robot
-            left_arm_poses = solution['left_arm_poses']
-
-            if type(left_arm_poses) == dict:
-                for joint, pose in left_arm_poses.items():
-                    robot.set_joint_state(joint, pose)
-            elif type(left_arm_poses) == str and left_arm_poses == "park":
-                _park_arms("left")
-
-            time.sleep(0.5)
+    def _execute(self, desig: MoveArmJointsMotion.Motion):
+        robot = BulletWorld.robot
+        if desig.left_arm_poses:
+            robot.set_joint_states(desig.left_arm_poses)
 
 
 class DonbotWorldStateDetecting(ProcessModule):
     """
     This process module detectes an object even if it is not in the field of view of the robot.
     """
 
-    def _execute(self, desig):
-        solution = desig.reference()
-        if solution['cmd'] == "world-state-detecting":
-            obj_type = solution['object_type']
-            return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0]
+    def _execute(self, desig: WorldStateDetectingMotion.Motion):
+        obj_type = desig.object_type
+        return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0]
+
+def _move_arm_tcp(target: Pose, robot: Object, arm: str) -> None:
+    gripper = robot_description.get_tool_frame(arm)
+
+    joints = robot_description.chains[arm].joints
+
+    inv = request_ik(target, robot, joints, gripper)
+    helper._apply_ik(robot, inv, joints)
 
 
 class DonbotManager(ProcessModuleManager):
 
     def __init__(self):
-        super().__init__("donbot")
+        super().__init__("iai_donbot")
         self._navigate_lock = Lock()
         self._pick_up_lock = Lock()
         self._place_lock = Lock()

src/pycram/process_modules/pr2_process_modules.py

@@ -72,7 +72,6 @@ def _execute(self, desig: PickUpMotion.Motion):
         target.orientation.w = grasp[3]
 
         arm = desig.arm
-        arm_short = "r" if arm == "right" else "l"
 
         _move_arm_tcp(target, robot, arm)
         tool_frame = robot_description.get_tool_frame(arm)

src/pycram/robot_descriptions/__init__.py

@@ -49,7 +49,7 @@ def update_robot_description(robot_name=None, from_ros=None):
         return None
 
     # Choose Description based on robot name
-    if 'donbot' in robot:
+    if 'iai_donbot' in robot:
         description = DonbotDescription
     elif 'pr2' in robot:
         description = PR2Description

src/pycram/robot_descriptions/boxy_description.py

@@ -28,13 +28,20 @@ def __init__(self):
         neck_behind = [-1.68, -0.26, -0.12, -0.24, 1.49, 3.09]
         # neck_forward = [-1.39, -3.09, -0.78, 1.91, 1.51, -0.14]
         neck_forward = [-1.20, 2.39, 0.17, 1.77, 1.44, -0.35]
-        neck_right = [-1.20, 2.39, 0.17, 1.77, 1.44, 0.0]
-        neck_left = [-1.20, 2.39, 0.17, 1.77, 1.44, -0.7]
+        # neck_right = [-1.20, 2.39, 0.17, 1.77, 1.44, 0.0]
+        # neck_left = [-1.20, 2.39, 0.17, 1.77, 1.44, -0.7]
         neck = ChainDescription("neck", neck_joints, neck_links, base_link=neck_base)
         neck.add_static_joint_chains({"away": neck_away, "down": neck_down, "down_left": neck_down_left,
                                       "down_right": neck_down_right, "behind_up": neck_behind_up, "behind": neck_behind,
-                                      "forward": neck_forward, "right": neck_right, "left": neck_left})
+                                      "forward": neck_forward})
         self.add_chain("neck", neck)
+        front = [-1.5, -1.57, 0, 1.58, -1.5, 0]
+        neck_right = [3.14, -1.57, 0, 1.58, -1.5, 0]
+        back = [1.5, -1.57, 0, 1.58, -1.5, 0]
+        neck_left = [0, -1.57, 0, 1.58, -1.5, 0]
+        self.add_static_joint_chains("neck", {"front": front, "neck_right": neck_right, "back": back,
+                                              "neck_left": neck_left})
+
         # Arms
         left_joints = ["left_arm_0_joint", "left_arm_1_joint", "left_arm_2_joint",
                        "left_arm_3_joint", "left_arm_4_joint", "left_arm_5_joint",