[AbstractWorld]

Added joint class to handle joint related things.
Object, Link, and Joint all have cached poses.
Cached poses update when World.simulate() is called.
Or when the poses are set manually.

src/pycram/bullet_world.py

@@ -143,14 +143,15 @@ def get_number_of_joints(self, obj: Object) -> int:
     def reset_joint_position(self, obj: Object, joint_name: str, joint_pose: float) -> None:
         p.resetJointState(obj.id, obj.joint_name_to_id[joint_name], joint_pose, physicsClientId=self.client_id)
 
-    def reset_object_base_pose(self, obj: Object, position: List[float], orientation: List[float]):
-        p.resetBasePositionAndOrientation(obj.id, position, orientation, physicsClientId=self.client_id)
+    def reset_object_base_pose(self, obj: Object, pose: Pose) -> None:
+        p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(),
+                                          physicsClientId=self.client_id)
 
     def step(self):
         p.stepSimulation(physicsClientId=self.client_id)
 
-    def update_obj_pose(self, obj: Object) -> None:
-        obj.set_pose(Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.client_id)))
+    def get_object_pose(self, obj: Object) -> Pose:
+        return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.client_id))
 
     def set_link_color(self, link: Link, rgba_color: Color):
         p.changeVisualShape(link.get_object_id(), link.id, rgbaColor=rgba_color, physicsClientId=self.client_id)

src/pycram/enums.py

@@ -19,7 +19,6 @@ class TaskStatus(Enum):
     SUCCEEDED = 2
     FAILED = 3
 
-
 class JointType(Enum):
     """
     Enum for readable joint types.
@@ -29,6 +28,9 @@ class JointType(Enum):
     SPHERICAL = 2
     PLANAR = 3
     FIXED = 4
+    UNKNOWN = 5
+    CONTINUOUS = 6
+    FLOATING = 7
 
 
 class Grasp(Enum):

src/pycram/pose_generator_and_validator.py

@@ -171,7 +171,7 @@ def reachability_validator(pose: Pose,
     if robot in allowed_collision.keys():
         allowed_robot_links = allowed_collision[robot]
 
-    joint_state_before_ik = robot._current_joints_positions
+    joint_state_before_ik = robot.get_positions_of_all_joints()
     try:
         # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints)
         resp = request_ik(target, robot, left_joints, left_gripper)