Merge pull request #175 from AbdelrhmanBassiouny/multiverse_goal_vali…

…dation

Multiverse goal validation

src/pycram/datastructures/pose.py

@@ -3,7 +3,7 @@
 
 import math
 import datetime
-from typing_extensions import List, Union, Optional
+from typing_extensions import List, Union, Optional, Sized
 
 import numpy as np
 import rospy
@@ -144,14 +144,17 @@ def orientation(self, value) -> None:
 
         :param value: New orientation, either a list or geometry_msgs/Quaternion
         """
-        if not isinstance(value, list) and not isinstance(value, tuple) and not isinstance(value, GeoQuaternion):
-            rospy.logwarn("Orientation can only be a list or geometry_msgs/Quaternion")
+        if not isinstance(value, Sized) and not isinstance(value, GeoQuaternion):
+            rospy.logwarn("Orientation can only be an iterable (list, tuple, ...etc.) or a geometry_msgs/Quaternion")
             return
 
-        if isinstance(value, list) or isinstance(value, tuple) and len(value) == 4:
+        if isinstance(value, Sized) and len(value) == 4:
             orientation = np.array(value)
-        else:
+        elif isinstance(value, GeoQuaternion):
             orientation = np.array([value.x, value.y, value.z, value.w])
+        else:
+            rospy.logerr("Orientation has to be a list or geometry_msgs/Quaternion")
+            raise TypeError("Orientation has to be a list or geometry_msgs/Quaternion")
         # This is used instead of np.linalg.norm since numpy is too slow on small arrays
         self.pose.orientation = get_normalized_quaternion(orientation)
 

src/pycram/description.py

@@ -63,8 +63,9 @@ class JointDescription(EntityDescription):
     A class that represents the description of a joint.
     """
 
-    def __init__(self, parsed_joint_description: Optional[Any] = None):
+    def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: bool = False):
         self.parsed_description = parsed_joint_description
+        self.is_virtual: Optional[bool] = is_virtual
 
     @property
     @abstractmethod
@@ -159,6 +160,13 @@ def __init__(self, _id: int, obj: Object):
         WorldEntity.__init__(self, _id, obj.world)
         self.object: Object = obj
 
+    @property
+    def object_name(self) -> str:
+        """
+        The name of the object to which this joint belongs.
+        """
+        return self.object.name
+
     @property
     @abstractmethod
     def pose(self) -> Pose:
@@ -227,6 +235,27 @@ def get_transform_from_root_link(self) -> Transform:
     def get_transform_to_root_link(self) -> Transform:
         return self.get_transform_to_link(self.object.root_link)
 
+    def set_pose(self, pose: Pose) -> None:
+        """
+        Sets the pose of this link to the given pose.
+        NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints
+        that can allow the link to be at the given pose.
+        :param pose: The target pose for this link.
+        """
+        self.object.set_pose(self.get_object_pose_given_link_pose(pose))
+
+    def get_object_pose_given_link_pose(self, pose):
+        return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose()
+
+    def get_pose_given_object_pose(self, pose):
+        return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose()
+
+    def get_transform_from_root_link(self) -> Transform:
+        return self.get_transform_from_link(self.object.root_link)
+
+    def get_transform_to_root_link(self) -> Transform:
+        return self.get_transform_to_link(self.object.root_link)
+
     @property
     def current_state(self) -> LinkState:
         return LinkState(self.constraint_ids.copy())
@@ -235,17 +264,18 @@ def current_state(self) -> LinkState:
     def current_state(self, link_state: LinkState) -> None:
         self.constraint_ids = link_state.constraint_ids
 
-    def add_fixed_constraint_with_link(self, child_link: 'Link', transform: Optional[Transform] = None) -> int:
+    def add_fixed_constraint_with_link(self, child_link: 'Link',
+                                       child_to_parent_transform: Optional[Transform] = None) -> int:
         """
         Add a fixed constraint between this link and the given link, to create attachments for example.
 
         :param child_link: The child link to which a fixed constraint should be added.
-        :param transform: The transformation between the two links.
+        :param child_to_parent_transform: The transformation between the two links.
         :return: The unique id of the constraint.
         """
-        if transform is None:
-            transform = child_link.get_transform_from_link(self)
-        constraint_id = self.world.add_fixed_constraint(self, child_link, transform)
+        if child_to_parent_transform is None:
+            child_to_parent_transform = child_link.get_transform_to_link(self)
+        constraint_id = self.world.add_fixed_constraint(self, child_link, child_to_parent_transform)
         self.constraint_ids[child_link] = constraint_id
         child_link.constraint_ids[self] = constraint_id
         return constraint_id
@@ -279,17 +309,6 @@ def is_root(self) -> bool:
         """
         return self.object.get_root_link_id() == self.id
 
-    def set_pose(self, pose: Pose) -> None:
-        """
-        Sets the pose of this link to the given pose.
-        NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints
-        that can allow the link to be at the given pose.
-        :param pose: The target pose for this link.
-        """
-        self.object.set_pose(
-            (pose.to_transform(self.tf_frame) * self.get_transform_to_link(self.object.root_link)).to_pose()
-        )
-
     def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None:
         """
         Update the transformation of this link at the given time.
@@ -473,9 +492,9 @@ class Joint(ObjectEntity, JointDescription, ABC):
 
     def __init__(self, _id: int,
                  joint_description: JointDescription,
-                 obj: Object):
+                 obj: Object, is_virtual: Optional[bool] = False):
         ObjectEntity.__init__(self, _id, obj)
-        JointDescription.__init__(self, joint_description.parsed_description)
+        JointDescription.__init__(self, joint_description.parsed_description, is_virtual)
         self._update_position()
 
     @property
@@ -634,7 +653,8 @@ def is_joint_virtual(self, name: str) -> bool:
     @abstractmethod
     def add_joint(self, name: str, child: str, joint_type: JointType,
                   axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None,
-                  lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None:
+                  lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                  is_virtual: Optional[bool] = False) -> None:
         """
         Add a joint to this object.
 
@@ -646,6 +666,7 @@ def add_joint(self, name: str, child: str, joint_type: JointType,
         :param origin: The origin of the joint.
         :param lower_limit: The lower limit of the joint.
         :param upper_limit: The upper limit of the joint.
+        :param is_virtual: True if the joint is virtual, False otherwise.
         """
         pass
 

src/pycram/designators/specialized_designators/location/giskard_location.py

@@ -53,7 +53,7 @@ def __iter__(self) -> CostmapLocation.Location:
                 prospection_robot = World.current_world.get_prospection_object_for_object(World.robot)
 
                 with UseProspectionWorld():
-                    prospection_robot.set_joint_positions(robot_joint_states)
+                    prospection_robot.set_multiple_joint_positions(robot_joint_states)
                     prospection_robot.set_pose(pose)
                     gripper_pose = prospection_robot.get_link_pose(chain.get_tool_frame())
 

src/pycram/external_interfaces/ik.py

@@ -234,7 +234,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[
             robot_joint_states[joint_name] = state
 
     with UseProspectionWorld():
-        prospection_robot.set_joint_positions(robot_joint_states)
+        prospection_robot.set_multiple_joint_positions(robot_joint_states)
         prospection_robot.set_pose(pose)
 
         tip_pose = prospection_robot.get_link_pose(gripper)

src/pycram/object_descriptors/urdf.py

@@ -82,8 +82,8 @@ class JointDescription(AbstractJointDescription):
 
     pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()}
 
-    def __init__(self, urdf_description: urdf.Joint):
-        super().__init__(urdf_description)
+    def __init__(self, urdf_description: urdf.Joint, is_virtual: Optional[bool] = False):
+        super().__init__(urdf_description, is_virtual=is_virtual)
 
     @property
     def origin(self) -> Pose:
@@ -177,7 +177,8 @@ class Joint(AbstractObjectDescription.Joint, JointDescription):
 
     def add_joint(self, name: str, child: str, joint_type: JointType,
                   axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None,
-                  lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None:
+                  lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                  is_virtual: Optional[bool] = False) -> None:
         if lower_limit is not None or upper_limit is not None:
             limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit)
         else:
@@ -196,6 +197,8 @@ def add_joint(self, name: str, child: str, joint_type: JointType,
                            JointDescription.pycram_type_map[joint_type],
                            axis, origin, limit)
         self.parsed_description.add_joint(joint)
+        if is_virtual:
+            self.virtual_joint_names.append(name)
 
     def load_description(self, path) -> URDF:
         with open(path, 'r') as file:

src/pycram/pose_generator_and_validator.py

@@ -203,22 +203,22 @@ def reachability_validator(pose: Pose,
             # test the possible solution and apply it to the robot
             pose, joint_states = request_ik(target, robot, joints, tool_frame)
             robot.set_pose(pose)
-            robot.set_joint_positions(joint_states)
+            robot.set_multiple_joint_positions(joint_states)
             # _apply_ik(robot, resp, joints)
 
             in_contact = collision_check(robot, allowed_collision)
             if not in_contact:  # only check for retract pose if pose worked
                 pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame)
                 robot.set_pose(pose)
-                robot.set_joint_positions(joint_states)
+                robot.set_multiple_joint_positions(joint_states)
                 # _apply_ik(robot, resp, joints)
                 in_contact = collision_check(robot, allowed_collision)
             if not in_contact:
                 arms.append(description.arm_type)
         except IKError:
             pass
         finally:
-            robot.set_joint_positions(joint_state_before_ik)
+            robot.set_multiple_joint_positions(joint_state_before_ik)
     if arms:
         res = True
     return res, arms

src/pycram/process_modules/boxy_process_modules.py

@@ -89,13 +89,13 @@ def _execute(self, desig):
         pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link"))
 
         if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y):
-            robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front"))
+            robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front"))
         if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x):
-            robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right"))
+            robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right"))
         if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y):
-            robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back"))
+            robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back"))
         if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x):
-            robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left"))
+            robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left"))
 
         pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link"))
 
@@ -115,7 +115,7 @@ def _execute(self, desig):
         robot = World.robot
         gripper = desig.gripper
         motion = desig.motion
-        robot.set_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion))
+        robot.set_multiple_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion))
 
 
 class BoxyDetecting(ProcessModule):
@@ -160,9 +160,9 @@ def _execute(self, desig: MoveArmJointsMotion):
 
         robot = World.robot
         if desig.right_arm_poses:
-            robot.set_joint_positions(desig.right_arm_poses)
+            robot.set_multiple_joint_positions(desig.right_arm_poses)
         if desig.left_arm_poses:
-            robot.set_joint_positions(desig.left_arm_poses)
+            robot.set_multiple_joint_positions(desig.left_arm_poses)
 
 
 class BoxyWorldStateDetecting(ProcessModule):

src/pycram/process_modules/donbot_process_modules.py

@@ -69,13 +69,13 @@ def _execute(self, desig):
         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_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front"))
+            robot.set_multiple_joint_positions(RobotDescription.current_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_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right"))
+            robot.set_multiple_joint_positions(RobotDescription.current_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_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back"))
+            robot.set_multiple_joint_positions(RobotDescription.current_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_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left"))
+            robot.set_multiple_joint_positions(RobotDescription.current_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"))
 
@@ -94,7 +94,7 @@ def _execute(self, desig):
         robot = World.robot
         gripper = desig.gripper
         motion = desig.motion
-        robot.set_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion))
+        robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion))
 
 
 class DonbotMoveTCP(ProcessModule):
@@ -118,7 +118,7 @@ class DonbotMoveJoints(ProcessModule):
     def _execute(self, desig: MoveArmJointsMotion):
         robot = World.robot
         if desig.left_arm_poses:
-            robot.set_joint_positions(desig.left_arm_poses)
+            robot.set_multiple_joint_positions(desig.left_arm_poses)
 
 
 class DonbotWorldStateDetecting(ProcessModule):

src/pycram/process_modules/pr2_process_modules.py

@@ -121,9 +121,9 @@ def _execute(self, desig: MoveArmJointsMotion):
 
         robot = World.robot
         if desig.right_arm_poses:
-            robot.set_joint_positions(desig.right_arm_poses)
+            robot.set_multiple_joint_positions(desig.right_arm_poses)
         if desig.left_arm_poses:
-            robot.set_joint_positions(desig.left_arm_poses)
+            robot.set_multiple_joint_positions(desig.left_arm_poses)
 
 
 class PR2MoveJoints(ProcessModule):
@@ -133,7 +133,7 @@ class PR2MoveJoints(ProcessModule):
 
     def _execute(self, desig: MoveJointsMotion):
         robot = World.robot
-        robot.set_joint_positions(dict(zip(desig.names, desig.positions)))
+        robot.set_multiple_joint_positions(dict(zip(desig.names, desig.positions)))
 
 
 class Pr2WorldStateDetecting(ProcessModule):

src/pycram/process_modules/stretch_process_modules.py

@@ -132,7 +132,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: Arms) -> None:
     # inv = request_ik(target, robot, joints, gripper)
     pose, joint_states = request_giskard_ik(target, robot, gripper)
     robot.set_pose(pose)
-    robot.set_joint_positions(joint_states)
+    robot.set_multiple_joint_positions(joint_states)
 
 
 ###########################################################

src/pycram/utils.py

@@ -46,7 +46,7 @@ def _apply_ik(robot: 'pycram.world_concepts.WorldObject', pose_and_joint_poses:
     """
     pose, joint_states = pose_and_joint_poses
     robot.set_pose(pose)
-    robot.set_joint_positions(joint_states)
+    robot.set_multiple_joint_positions(joint_states)
 
 
 class GeneratorList: