Merge pull request #100 from Tigul/transforn-optimization

Transforn optimization

doc/source/installation.rst

@@ -14,6 +14,9 @@ All dependencies are available via PyPi.
 
 PyCRAM is developed and tested currently with Python3.8, Ubuntu 20.04 and ROS Noetic.
 
+This guide excpects you to have a GitHub account with an SSH key (you can read about adding a new ssh key
+`here <https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account>`_).
+
 Installing ROS
 ==============
 
@@ -66,6 +69,8 @@ Now you can install PyCRAM into your ROS workspace.
     rosdep install --ignore-src --from-paths . -r
     cd ..
     catkin_make
+    source devel/setup.bash
+    echo "~/workspace/ros/devel/setup.bash" >> ~/.bashrc
 
 The cloning and setting up can take several minutes. After the command finishes you should see a number of repositories
 in your ROS workspace.
@@ -112,13 +117,15 @@ Building and sourcing your ROS workspace using catkin compiles all ROS packages
 respective PATH variables. This is necessary to be able to import PyCRAM via the Python import system and to find the
 robot descriptions in the launch file.
 
+If you have been following the tutorial steps until now you can skip this part. 
+
 You can build your ROS workspace with the following commands:
 
 .. code-block:: console
 
     cd ~/workspace/ros
     catkin_make
-    source devel/local_setup.bash
+    source devel/setup.bash
 
 Using PyCRAM
 ============

src/pycram/bullet_world.py

@@ -371,8 +371,9 @@ def reset_bullet_world(self) -> None:
                 attached_objects = list(obj.attachments.keys())
                 for att_obj in attached_objects:
                     obj.detach(att_obj)
-            for joint_name in obj.joints.keys():
-                obj.set_joint_state(joint_name, 0)
+            joint_names = list(obj.joints.keys())
+            joint_poses = [0 for j in joint_names]
+            obj.set_joint_states(dict(zip(joint_names, joint_poses)))
             obj.set_pose(obj.original_pose)
 
 
@@ -462,14 +463,17 @@ def run(self):
                 self.remove_obj_queue.task_done()
 
             for bulletworld_obj, shadow_obj in self.object_mapping.items():
-                shadow_obj.set_pose(bulletworld_obj.get_pose())
-                # shadow_obj.set_position(bulletworld_obj.get_position())
-                # shadow_obj.set_orientation(bulletworld_obj.get_orientation())
+                b_pose = bulletworld_obj.get_pose()
+                s_pose = shadow_obj.get_pose()
+                if b_pose.dist(s_pose) != 0.0:
+                    shadow_obj.set_pose(bulletworld_obj.get_pose())
 
                 # Manage joint positions
                 if len(bulletworld_obj.joints) > 2:
                     for joint_name in bulletworld_obj.joints.keys():
-                        shadow_obj.set_joint_state(joint_name, bulletworld_obj.get_joint_state(joint_name))
+                        if shadow_obj.get_joint_state(joint_name) != bulletworld_obj.get_joint_state(joint_name):
+                            shadow_obj.set_joint_states(bulletworld_obj.get_complete_joint_state())
+                            break
 
             self.check_for_pause()
             # self.check_for_equal()
@@ -518,10 +522,11 @@ def run(self):
         else:
             self.world.client_id = p.connect(p.GUI)
 
-            #Disable the side windows of the GUI
+            # Disable the side windows of the GUI
             p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
             # Change the init camera pose
-            p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50, cameraTargetPosition=[-2, 0, 1])
+            p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=270.0, cameraPitch=-50,
+                                         cameraTargetPosition=[-2, 0, 1])
 
             # Get the initial camera target location
             cameraTargetPosition = p.getDebugVisualizerCamera()[11]
@@ -746,7 +751,6 @@ def __init__(self, name: str, type: str, path: str,
         self.attachments: Dict[Object, List] = {}
         self.cids: Dict[Object, int] = {}
         self.original_pose = pose_in_map
-        self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list())
 
         self.tf_frame = ("shadow/" if self.world.is_shadow_world else "") + self.name + "_" + str(self.id)
 
@@ -758,9 +762,18 @@ def __init__(self, name: str, type: str, path: str,
         with open(self.path) as f:
             self.urdf_object = URDF.from_xml_string(f.read())
             if self.urdf_object.name == robot_description.name and not BulletWorld.robot:
-                    BulletWorld.robot = self
+                BulletWorld.robot = self
 
         self.links[self.urdf_object.get_root()] = -1
+
+        self._current_pose = pose_in_map
+        self._current_link_poses = {}
+        self._current_link_transforms = {}
+        self._current_joint_states = {}
+        self._init_current_joint_states()
+        self._update_link_poses()
+
+        self.base_origin_shift = np.array(position) - np.array(self.get_base_origin().position_as_list())
         self.local_transformer.update_transforms_for_object(self)
         self.link_to_geometry = self._get_geometry_for_link()
 
@@ -866,7 +879,7 @@ def get_pose(self) -> Pose:
 
         :return: The current pose of this object
         """
-        return Pose(*p.getBasePositionAndOrientation(self.id, physicsClientId=self.world.client_id))
+        return self._current_pose
 
     def set_pose(self, pose: Pose, base: bool = False) -> None:
         """
@@ -880,7 +893,8 @@ def set_pose(self, pose: Pose, base: bool = False) -> None:
         if base:
             position = np.array(position) + self.base_origin_shift
         p.resetBasePositionAndOrientation(self.id, position, orientation, self.world.client_id)
-        self.local_transformer.update_transforms_for_object(self)
+        self._current_pose = pose_in_map
+        self._update_link_poses()
         self._set_attached_objects([self])
 
     @property
@@ -942,6 +956,7 @@ def _set_attached_objects(self, prev_object: List[Object]) -> None:
                 p.resetBasePositionAndOrientation(obj.id, world_to_object.position_as_list(),
                                                   world_to_object.orientation_as_list(),
                                                   physicsClientId=self.world.client_id)
+                obj._current_pose = world_to_object
                 obj._set_attached_objects(prev_object + [self])
 
     def _calculate_transform(self, obj: Object, link: str = None) -> Transform:
@@ -1084,7 +1099,8 @@ def get_link_pose(self, name: str) -> Pose:
         """
         if name in self.links.keys() and self.links[name] == -1:
             return self.get_pose()
-        return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6])
+        return self._current_link_poses[name]
+        # return Pose(*p.getLinkState(self.id, self.links[name], physicsClientId=self.world.client_id)[4:6])
 
     def set_joint_state(self, joint_name: str, joint_pose: float) -> None:
         """
@@ -1106,7 +1122,23 @@ def set_joint_state(self, joint_name: str, joint_pose: float) -> None:
             # Temporarily disabled because kdl outputs values exciting joint limits
             # return
         p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id)
-        self.local_transformer.update_transforms_for_object(self)
+        self._current_joint_states[joint_name] = joint_pose
+        # self.local_transformer.update_transforms_for_object(self)
+        self._update_link_poses()
+        self._set_attached_objects([self])
+
+    def set_joint_states(self, joint_poses: dict) -> None:
+        """
+        Sets the current state of multiple joints at once, this method should be preferred when setting multiple joints
+        at once instead of running :func:`~Object.set_joint_state` in a loop.
+
+        :param joint_poses:
+        :return:
+        """
+        for joint_name, joint_pose in joint_poses.items():
+            p.resetJointState(self.id, self.joints[joint_name], joint_pose, physicsClientId=self.world.client_id)
+            self._current_joint_states[joint_name] = joint_pose
+        self._update_link_poses()
         self._set_attached_objects([self])
 
     def get_joint_state(self, joint_name: str) -> float:
@@ -1116,7 +1148,7 @@ def get_joint_state(self, joint_name: str) -> float:
         :param joint_name: The name of the joint
         :return: The current pose of the joint
         """
-        return p.getJointState(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[0]
+        return self._current_joint_states[joint_name]
 
     def contact_points(self) -> List:
         """
@@ -1317,10 +1349,7 @@ def get_complete_joint_state(self) -> Dict[str: float]:
 
         :return: A dictionary with the complete joint state
         """
-        result = {}
-        for joint in self.joints.keys():
-            result[joint] = self.get_joint_state(joint)
-        return result
+        return self._current_joint_states
 
     def get_link_tf_frame(self, link_name: str) -> str:
         """
@@ -1347,6 +1376,28 @@ def _get_geometry_for_link(self) -> Dict[str, urdf_parser_py.urdf.Geometry]:
                 link_to_geometry[link] = link_obj.collision.geometry
         return link_to_geometry
 
+    def _update_link_poses(self) -> None:
+        """
+        Updates the cached poses and transforms for each link of this Object
+        """
+        for link_name in self.links.keys():
+            if link_name == self.urdf_object.get_root():
+                self._current_link_poses[link_name] = self._current_pose
+                self._current_link_transforms[link_name] = self._current_pose.to_transform(self.tf_frame)
+            else:
+                self._current_link_poses[link_name] = Pose(*p.getLinkState(self.id, self.links[link_name],
+                                                                           physicsClientId=self.world.client_id)[4:6])
+                self._current_link_transforms[link_name] = self._current_link_poses[link_name].to_transform(
+                    self.get_link_tf_frame(link_name))
+
+    def _init_current_joint_states(self) -> None:
+        """
+        Initialize the cached joint position for each joint.
+        """
+        for joint_name in self.joints.keys():
+            self._current_joint_states[joint_name] = \
+                p.getJointState(self.id, self.joints[joint_name], physicsClientId=self.world.client_id)[0]
+
 
 def filter_contact_points(contact_points, exclude_ids) -> List:
     """

src/pycram/helper.py

@@ -45,6 +45,7 @@ class bcolors:
 def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[str]) -> None:
     """
     Apllies a list of joint poses calculated by an inverse kinematics solver to a robot
+
     :param robot: The robot the joint poses should be applied on
     :param joint_poses: The joint poses to be applied
     :param gripper: specifies the gripper for which the ik solution should be applied
@@ -53,8 +54,9 @@ def _apply_ik(robot: BulletWorldObject, joint_poses: List[float], joints: List[s
     # arm ="left" if gripper == robot_description.get_tool_frame("left") else "right"
     # ik_joints = [robot_description.torso_joint] + robot_description._safely_access_chains(arm).joints
     # ik_joints = robot_description._safely_access_chains(arm).joints
-    for i in range(0, len(joints)):
-        robot.set_joint_state(joints[i], joint_poses[i])
+    robot.set_joint_states(dict(zip(joints, joint_poses)))
+    # for i in range(0, len(joints)):
+    #     robot.set_joint_state(joints[i], joint_poses[i])
 
 
 def _transform_to_torso(pose_and_rotation: Tuple[List[float], List[float]], robot: BulletWorldObject) -> Tuple[

src/pycram/local_transformer.py

@@ -57,13 +57,13 @@ def __init__(self):
         # If the singelton was already initialized
         self._initialized = True
 
-    def update_objects(self) -> None:
+    def update_objects_for_current_world(self) -> None:
         """
         Updates transformations for all objects that are currently in :py:attr:`~pycram.bullet_world.BulletWorld.current_bullet_world`
         """
-        if self.bullet_world:
-            for obj in list(self.bullet_world.current_bullet_world.objects):
-                self.update_transforms_for_object(obj)
+        curr_time = rospy.Time.now()
+        for obj in list(self.bullet_world.current_bullet_world.objects):
+            self.update_transforms_for_object(obj, curr_time)
 
     def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]:
         """
@@ -73,6 +73,7 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]:
         :param target_frame: Name of the TF frame into which the Pose should be transformed
         :return: A transformed pose in the target frame
         """
+        self.update_objects_for_current_world()
         copy_pose = pose.copy()
         copy_pose.header.stamp = rospy.Time(0)
         if not self.canTransform(target_frame, pose.frame, rospy.Time(0)):
@@ -115,24 +116,22 @@ def tf_transform(self, source_frame: str, target_frame: str,
         :param time: Time at which the transform should be
         :return:
         """
+        self.update_objects_for_current_world()
         tf_time = time if time else self.getLatestCommonTime(source_frame, target_frame)
         translation, rotation = self.lookupTransform(source_frame, target_frame, tf_time)
         return Transform(translation, rotation, source_frame, target_frame)
 
-    def update_transforms_for_object(self, bullet_object: 'bullet_world.Object') -> None:
+    def update_transforms_for_object(self, bullet_object: 'bullet_world.Object', time: rospy.Time = None) -> None:
         """
         Updates local transforms for a Bullet Object, this includes the base as well as all links
 
         :param bullet_object: Object for which the Transforms should be updated
+        :param time: a specific time that should be used
         """
-        self.setTransform(
-            bullet_object.get_pose().to_transform(bullet_object.tf_frame))
-        for link_name, id in bullet_object.links.items():
-            if id == -1:
-                continue
-            tf_stamped = bullet_object.get_link_pose(link_name).to_transform(
-                bullet_object.get_link_tf_frame(link_name))
-            self.setTransform(tf_stamped)
+        time = time if time else rospy.Time.now()
+        for transform in bullet_object._current_link_transforms.values():
+            transform.header.stamp = time
+            self.setTransform(transform)
 
     def get_all_frames(self) -> List[str]:
         """