def __init__(self): self.time_step = 0.004 self.finger = sim_finger.SimFinger(finger_type=Cage.args.finger_type, time_step=self.time_step, enable_visualization=True,) self.num_fingers = self.finger.number_of_fingers _kwargs = {"physicsClientId": self.finger._pybullet_client_id} if Cage.args.control_mode == "position": self.position_goals = visual_objects.Marker(number_of_goals=self.num_fingers) self.initial_robot_position=np.array([0.0, np.deg2rad(-60), np.deg2rad(-60)] * 3, dtype=np.float32,) self.finger.reset_finger_positions_and_velocities(self.initial_robot_position) self.initial_object_pose = move_cube.Pose(position=[0, 0, move_cube._min_height], orientation=move_cube.Pose().orientation) self.cube = collision_objects.Block(self.initial_object_pose.position, self.initial_object_pose.orientation, mass=0.020, **_kwargs) self.cube_z = 0.0325 self.workspace_radius = 0.18 self.tricamera = camera.TriFingerCameras(**_kwargs) self.initial_marker_position = np.array([[ 0. , 0.17856407, self.cube_z], [ 0.15464102, -0.08928203, self.cube_z], [-0.15464102, -0.08928203, self.cube_z]]) self.position_goals.set_state(self.initial_marker_position) self.marker_position = self.initial_marker_position self.ik_module = geometric_ik.GeometricIK()
def test_evaluate_state_difficulty_4(self): difficulty = 4 pose_origin = move_cube.Pose() pose_trans = move_cube.Pose(position=[1, 2, 3]) pose_rot = move_cube.Pose( orientation=Rotation.from_euler("z", 0.42).as_quat() ) pose_both = move_cube.Pose( [1, 2, 3], Rotation.from_euler("z", 0.42).as_quat() ) # needs to be zero for exact match cost = move_cube.evaluate_state(pose_origin, pose_origin, difficulty) self.assertEqual(cost, 0) # None-zero if there is translation, rotation or both self.assertNotEqual( move_cube.evaluate_state(pose_origin, pose_trans, difficulty), 0 ) self.assertNotEqual( move_cube.evaluate_state(pose_origin, pose_rot, difficulty), 0 ) self.assertNotEqual( move_cube.evaluate_state(pose_origin, pose_both, difficulty), 0 )
def compute_reward(self, achieved_goal, desired_goal, info): """Compute the reward for the given achieved and desired goal. Args: achieved_goal : Current pose of the object. desired_goal : Goal pose of the object. info : An info dictionary containing a field "difficulty" which specifies the difficulty level. Returns: float: The reward that corresponds to the provided achieved goal w.r.t. to the desired goal. Note that the following should always hold true:: ob, reward, done, info = env.step() assert reward == env.compute_reward( ob['achieved_goal'], ob['desired_goal'], info, ) """ if self.sparse_reward: return -np.float32((move_cube.evaluate_state( move_cube.Pose(desired_goal[0:3], desired_goal[3:7]), move_cube.Pose(achieved_goal[0:3], achieved_goal[3:7]), info['difficulty'], ) > 0.01)) else: return move_cube.evaluate_state( move_cube.Pose.from_dict(desired_goal), move_cube.Pose.from_dict(achieved_goal), info['difficulty'], )
def _competition_reward(observation, difficulty): object_pose = move_cube.Pose( observation['object_position'], observation['object_orientation'] ) goal_pose = move_cube.Pose( observation['goal_object_position'], observation['goal_object_orientation'] ) return -move_cube.evaluate_state(goal_pose, object_pose, difficulty)
def reset(self): # reset simulation del self.platform # initialize simulation if self.initializer is None: # if no initializer is given (which will be the case during training), # we can initialize in any way desired. here, we initialize the cube always # in the center of the arena, instead of randomly, as this appears to help # training initial_robot_position = TriFingerPlatform.spaces.robot_position.default default_object_position = ( TriFingerPlatform.spaces.object_position.default) default_object_orientation = ( TriFingerPlatform.spaces.object_orientation.default) initial_object_pose = move_cube.Pose( position=default_object_position, orientation=default_object_orientation, ) goal_object_pose = move_cube.sample_goal(difficulty=1) else: # if an initializer is given, i.e. during evaluation, we need to initialize # according to it, to make sure we remain coherent with the standard CubeEnv. # otherwise the trajectories produced during evaluation will be invalid. initial_robot_position = TriFingerPlatform.spaces.robot_position.default initial_object_pose = self.initializer.get_initial_state() goal_object_pose = self.initializer.get_goal() self.platform = TriFingerPlatform( visualization=self.visualization, initial_robot_position=initial_robot_position, initial_object_pose=initial_object_pose, ) self.goal = { "position": goal_object_pose.position, "orientation": goal_object_pose.orientation, } # visualize the goal if self.visualization: self.goal_marker = visual_objects.CubeMarker( width=0.065, position=goal_object_pose.position, orientation=goal_object_pose.orientation, physicsClientId=self.platform.simfinger._pybullet_client_id, ) self.info = {"difficulty": 1} self.step_count = 0 return self._create_observation(0)
def test_validate_goal(self): half_width = move_cube._CUBE_WIDTH / 2 yaw_rotation = Rotation.from_euler("z", 0.42).as_quat() full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat() # test some valid goals try: move_cube.validate_goal( move_cube.Pose([0, 0, half_width], [0, 0, 0, 1]) ) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) try: move_cube.validate_goal( move_cube.Pose([0.05, -0.1, half_width], yaw_rotation) ) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) try: move_cube.validate_goal( move_cube.Pose([-0.12, 0.0, 0.06], full_rotation) ) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) # test some invalid goals # invalid values with self.assertRaises(ValueError): move_cube.validate_goal(move_cube.Pose([0, 0], [0, 0, 0, 1])) with self.assertRaises(ValueError): move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 1])) # invalid positions with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0.3, 0, half_width], [0, 0, 0, 1]) ) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, -0.3, half_width], [0, 0, 0, 1]) ) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, 0.3], [0, 0, 0, 1])) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 0, 1])) with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, 0, -0.01], [0, 0, 0, 1]) ) # valid CoM position but rotation makes it reach out of valid range with self.assertRaises(move_cube.InvalidGoalError): move_cube.validate_goal( move_cube.Pose([0, 0, half_width], full_rotation) )
def test_get_cube_corner_positions(self): # cube half width chw = move_cube._CUBE_WIDTH / 2 # no transformation expected_origin_corners = np.array( [ [-chw, -chw, -chw], [-chw, -chw, +chw], [-chw, +chw, -chw], [-chw, +chw, +chw], [+chw, -chw, -chw], [+chw, -chw, +chw], [+chw, +chw, -chw], [+chw, +chw, +chw], ] ) origin = move_cube.Pose() origin_corners = move_cube.get_cube_corner_positions(origin) np.testing.assert_array_almost_equal( expected_origin_corners, origin_corners ) # only translation expected_translated_corners = np.array( [ [-chw + 1, -chw + 2, -chw + 3], [-chw + 1, -chw + 2, +chw + 3], [-chw + 1, +chw + 2, -chw + 3], [-chw + 1, +chw + 2, +chw + 3], [+chw + 1, -chw + 2, -chw + 3], [+chw + 1, -chw + 2, +chw + 3], [+chw + 1, +chw + 2, -chw + 3], [+chw + 1, +chw + 2, +chw + 3], ] ) translated = move_cube.get_cube_corner_positions( move_cube.Pose([1, 2, 3], [0, 0, 0, 1]) ) np.testing.assert_array_almost_equal( expected_translated_corners, translated ) # only rotation rot_z90 = Rotation.from_euler("z", 90, degrees=True).as_quat() expected_rotated_corners = np.array( [ [+chw, -chw, -chw], [+chw, -chw, +chw], [-chw, -chw, -chw], [-chw, -chw, +chw], [+chw, +chw, -chw], [+chw, +chw, +chw], [-chw, +chw, -chw], [-chw, +chw, +chw], ] ) rotated = move_cube.get_cube_corner_positions( move_cube.Pose([0, 0, 0], rot_z90) ) np.testing.assert_array_almost_equal(expected_rotated_corners, rotated) # both rotation and translation expected_both_corners = np.array( [ [+chw + 1, -chw + 2, -chw + 3], [+chw + 1, -chw + 2, +chw + 3], [-chw + 1, -chw + 2, -chw + 3], [-chw + 1, -chw + 2, +chw + 3], [+chw + 1, +chw + 2, -chw + 3], [+chw + 1, +chw + 2, +chw + 3], [-chw + 1, +chw + 2, -chw + 3], [-chw + 1, +chw + 2, +chw + 3], ] ) both = move_cube.get_cube_corner_positions( move_cube.Pose([1, 2, 3], rot_z90) ) np.testing.assert_array_almost_equal(expected_both_corners, both)
class TriFingerPlatform: """ Wrapper around the simulation providing the same interface as ``robot_interfaces::TriFingerPlatformFrontend``. The following methods of the robot_interfaces counterpart are not supported: - get_robot_status() - wait_until_timeindex() """ # Create the action and observation spaces # ======================================== _n_joints = 9 _n_fingers = 3 _max_torque_Nm = 0.36 _max_velocity_radps = 10 spaces = SimpleNamespace() spaces.robot_torque = SimpleNamespace( low=np.full(_n_joints, -_max_torque_Nm, dtype=np.float32), high=np.full(_n_joints, _max_torque_Nm, dtype=np.float32), default=np.zeros(_n_joints, dtype=np.float32), ) spaces.robot_position = SimpleNamespace( low=np.array([-0.9, -1.57, -2.7] * _n_fingers, dtype=np.float32), high=np.array([1.4, 1.57, 0.0] * _n_fingers, dtype=np.float32), default=np.array( [0.0, np.deg2rad(70), np.deg2rad(-130)] * _n_fingers, dtype=np.float32, ), ) spaces.robot_velocity = SimpleNamespace( low=np.full(_n_joints, -_max_velocity_radps, dtype=np.float32), high=np.full(_n_joints, _max_velocity_radps, dtype=np.float32), default=np.zeros(_n_joints, dtype=np.float32), ) spaces.object_position = SimpleNamespace( low=np.array([-0.3, -0.3, 0], dtype=np.float32), high=np.array([0.3, 0.3, 0.3], dtype=np.float32), default=np.array([0, 0, move_cube._min_height], dtype=np.float32), ) spaces.object_orientation = SimpleNamespace( low=-np.ones(4, dtype=np.float32), high=np.ones(4, dtype=np.float32), default=move_cube.Pose().orientation, ) # for convenience, we also create the respective gym spaces spaces.robot_torque.gym = gym.spaces.Box(low=spaces.robot_torque.low, high=spaces.robot_torque.high) spaces.robot_position.gym = gym.spaces.Box(low=spaces.robot_position.low, high=spaces.robot_position.high) spaces.robot_velocity.gym = gym.spaces.Box(low=spaces.robot_velocity.low, high=spaces.robot_velocity.high) spaces.object_position.gym = gym.spaces.Box( low=spaces.object_position.low, high=spaces.object_position.high) spaces.object_orientation.gym = gym.spaces.Box( low=spaces.object_orientation.low, high=spaces.object_orientation.high) def __init__( self, visualization=False, initial_robot_position=None, initial_object_pose=None, enable_cameras=False, ): """Initialize. Args: visualization (bool): Set to true to run visualization. initial_robot_position: Initial robot joint angles initial_object_pose: Initial pose for the manipulation object. Can be any object with attributes ``position`` (x, y, z) and ``orientation`` (x, y, z, w). This is optional, if not set, a random pose will be sampled. enable_cameras (bool): Set to true to enable camera observations. By default this is disabled as rendering of images takes a lot of computational power. Therefore the cameras should only be enabled if the images are actually used. """ #: Camera rate in frames per second. Observations of camera and #: object pose will only be updated with this rate. #: NOTE: This is currently not used! self._camera_rate_fps = 30 #: Set to true to render camera observations self.enable_cameras = enable_cameras #: Simulation time step self._time_step = 0.004 # first camera update in the first step self._next_camera_update_step = 0 # Initialize robot, object and cameras # ==================================== self.simfinger = SimFinger( finger_type="trifingerpro", time_step=self._time_step, enable_visualization=visualization, ) if initial_robot_position is None: initial_robot_position = self.spaces.robot_position.default self.simfinger.reset_finger_positions_and_velocities( initial_robot_position) if initial_object_pose is None: initial_object_pose = move_cube.Pose( position=self.spaces.object_position.default, orientation=self.spaces.object_orientation.default, ) self.cube = collision_objects.Block( initial_object_pose.position, initial_object_pose.orientation, mass=0.020, ) self.tricamera = camera.TriFingerCameras() # Forward some methods for convenience # ==================================== # forward "RobotFrontend" methods directly to simfinger self.Action = self.simfinger.Action self.get_desired_action = self.simfinger.get_desired_action self.get_applied_action = self.simfinger.get_applied_action self.get_timestamp_ms = self.simfinger.get_timestamp_ms self.get_current_timeindex = self.simfinger.get_current_timeindex self.get_robot_observation = self.simfinger.get_observation # forward kinematics directly to simfinger self.forward_kinematics = ( self.simfinger.pinocchio_utils.forward_kinematics) # Initialize log # ============== self._action_log = { "initial_robot_position": initial_robot_position.tolist(), "initial_object_pose": { "position": initial_object_pose.position.tolist(), "orientation": initial_object_pose.orientation.tolist(), }, "actions": [], } def get_time_step(self): """Get simulation time step in seconds.""" return self._time_step def _compute_camera_update_step_interval(self): return (1.0 / self._camera_rate_fps) / self._time_step def append_desired_action(self, action): """ Call :meth:`pybullet.SimFinger.append_desired_action` and add the action to the action log. Arguments/return value are the same as for :meth:`pybullet.SimFinger.append_desired_action`. """ # update camera and object observations only with the rate of the # cameras # next_t = self.get_current_timeindex() + 1 # has_camera_update = next_t >= self._next_camera_update_step # if has_camera_update: # self._next_camera_update_step += ( # self._compute_camera_update_step_interval() # ) # self._object_pose_t = self._get_current_object_pose() # if self.enable_cameras: # self._camera_observation_t = ( # self._get_current_camera_observation() # ) has_camera_update = True self._object_pose_t = self._get_current_object_pose() if self.enable_cameras: self._camera_observation_t = self._get_current_camera_observation() t = self.simfinger.append_desired_action(action) # The correct timestamp can only be acquired now that t is given. # Update it accordingly in the object and camera observations if has_camera_update: camera_timestamp_s = self.get_timestamp_ms(t) / 1000 self._object_pose_t.timestamp = camera_timestamp_s if self.enable_cameras: for i in range(len(self._camera_observation_t.cameras)): self._camera_observation_t.cameras[ i].timestamp = camera_timestamp_s # write the desired action to the log self._action_log["actions"].append({ "t": t, "torque": action.torque.tolist(), "position": action.position.tolist(), "position_kp": action.position_kp.tolist(), "position_kd": action.position_kd.tolist(), }) return t def _get_current_object_pose(self, t=None): cube_state = self.cube.get_state() pose = ObjectPose() pose.position = np.asarray(cube_state[0]) pose.orientation = np.asarray(cube_state[1]) pose.confidence = 1.0 # NOTE: The timestamp can only be set correctly after time step t is # actually reached. Therefore, this is set to None here and filled # with the proper value later. if t is None: pose.timestamp = None else: pose.timestamp = self.get_timestamp_ms(t) return pose def get_object_pose(self, t): """Get object pose at time step t. Args: t: The time index of the step for which the object pose is requested. Only the value returned by the last call of :meth:`~append_desired_action` is valid. Returns: ObjectPose: Pose of the object. Values come directly from the simulation without adding noise, so the confidence is 1.0. Raises: ValueError: If invalid time index ``t`` is passed. """ current_t = self.simfinger._t if t < 0: raise ValueError("Cannot access time index less than zero.") elif t == current_t: return self._object_pose_t elif t == current_t + 1: return self._get_current_object_pose(t) else: raise ValueError( "Given time index t has to match with index of the current" " step or the next one.") def _get_current_camera_observation(self, t=None): images = self.tricamera.get_images() observation = TriCameraObservation() # NOTE: The timestamp can only be set correctly after time step t # is actually reached. Therefore, this is set to None here and # filled with the proper value later. if t is None: timestamp = None else: timestamp = self.get_timestamp_ms(t) for i, image in enumerate(images): observation.cameras[i].image = image observation.cameras[i].timestamp = timestamp return observation def get_camera_observation(self, t): """Get camera observation at time step t. Args: t: The time index of the step for which the observation is requested. Only the value returned by the last call of :meth:`~append_desired_action` is valid. Returns: TriCameraObservation: Observations of the three cameras. Images are rendered in the simulation. Note that they are not optimized to look realistically. Raises: ValueError: If invalid time index ``t`` is passed. """ if not self.enable_cameras: raise RuntimeError( "Cameras are not enabled. Create `TriFingerPlatform` with" " `enable_cameras=True` if you want to use camera" " observations.") current_t = self.simfinger._t if t < 0: raise ValueError("Cannot access time index less than zero.") elif t == current_t: return self._camera_observation_t elif t == current_t + 1: return self._get_current_camera_observation(t) else: raise ValueError( "Given time index t has to match with index of the current" " step or the next one.") def store_action_log(self, filename): """Store the action log to a JSON file. Args: filename (str): Path to the JSON file to which the log shall be written. If the file exists already, it will be overwritten. """ # TODO should the log also contain intermediate observations (object # and finger) for verification? t = self.simfinger.get_current_timeindex() object_pose = self.get_object_pose(t) self._action_log["final_object_pose"] = { "position": object_pose.position.tolist(), "orientation": object_pose.orientation.tolist(), } with open(filename, "w") as fh: json.dump(self._action_log, fh)
def __init__( self, visualization=False, initial_robot_position=None, initial_object_pose=None, enable_cameras=False, ): """Initialize. Args: visualization (bool): Set to true to run visualization. initial_robot_position: Initial robot joint angles initial_object_pose: Initial pose for the manipulation object. Can be any object with attributes ``position`` (x, y, z) and ``orientation`` (x, y, z, w). This is optional, if not set, a random pose will be sampled. enable_cameras (bool): Set to true to enable camera observations. By default this is disabled as rendering of images takes a lot of computational power. Therefore the cameras should only be enabled if the images are actually used. """ #: Camera rate in frames per second. Observations of camera and #: object pose will only be updated with this rate. #: NOTE: This is currently not used! self._camera_rate_fps = 30 #: Set to true to render camera observations self.enable_cameras = enable_cameras #: Simulation time step self._time_step = 0.004 # first camera update in the first step self._next_camera_update_step = 0 # Initialize robot, object and cameras # ==================================== self.simfinger = SimFinger( finger_type="trifingerpro", time_step=self._time_step, enable_visualization=visualization, ) if initial_robot_position is None: initial_robot_position = self.spaces.robot_position.default self.simfinger.reset_finger_positions_and_velocities( initial_robot_position) if initial_object_pose is None: initial_object_pose = move_cube.Pose( position=self.spaces.object_position.default, orientation=self.spaces.object_orientation.default, ) self.cube = collision_objects.Block( initial_object_pose.position, initial_object_pose.orientation, mass=0.020, ) self.tricamera = camera.TriFingerCameras() # Forward some methods for convenience # ==================================== # forward "RobotFrontend" methods directly to simfinger self.Action = self.simfinger.Action self.get_desired_action = self.simfinger.get_desired_action self.get_applied_action = self.simfinger.get_applied_action self.get_timestamp_ms = self.simfinger.get_timestamp_ms self.get_current_timeindex = self.simfinger.get_current_timeindex self.get_robot_observation = self.simfinger.get_observation # forward kinematics directly to simfinger self.forward_kinematics = ( self.simfinger.pinocchio_utils.forward_kinematics) # Initialize log # ============== self._action_log = { "initial_robot_position": initial_robot_position.tolist(), "initial_object_pose": { "position": initial_object_pose.position.tolist(), "orientation": initial_object_pose.orientation.tolist(), }, "actions": [], }