def __init__(self, count: int, name: str, joint_names: List[str]): super().__init__(count, name, joint_names) suffix = '' if count == 0 else '#%d' % (count - 1) self._touch_sensors = [] i = 0 while True: fname = '%s_touchSensor%d%s' % (name, i, suffix) if not ForceSensor.exists(fname): break self._touch_sensors.append(ForceSensor(fname)) i += 1
def to_type(handle: int) -> Object: """Converts an object handle to the correct sub-type. :param handle: The internal handle of an object. :return: The sub-type of this object. """ t = sim.simGetObjectType(handle) if t == sim.sim_object_shape_type: return Shape(handle) elif t == sim.sim_object_dummy_type: return Dummy(handle) elif t == sim.sim_object_path_type: return CartesianPath(handle) elif t == sim.sim_object_joint_type: return Joint(handle) elif t == sim.sim_object_visionsensor_type: return VisionSensor(handle) elif t == sim.sim_object_forcesensor_type: return ForceSensor(handle) elif t == sim.sim_object_proximitysensor_type: return ProximitySensor(handle) elif t == sim.sim_object_camera_type: return Camera(handle) elif t == sim.sim_object_octree_type: return Octree(handle) raise ValueError
def get_low_dim_state(self) -> np.ndarray: """Gets the pose and various other properties of objects in the task. :return: 1D array of low-dimensional task state. """ # Corner cases: # (1) Object has been deleted. # (2) Object has been grasped (and is now child of gripper). state = [] for obj, objtype in self._initial_objs_in_scene: if not obj.still_exists(): # It has been deleted empty_len = 7 if objtype == ObjectType.JOINT: empty_len += 1 elif objtype == ObjectType.FORCE_SENSOR: empty_len += 6 state.extend(np.zeros((empty_len, )).tolist()) else: state.extend(np.array(obj.get_pose())) if obj.get_type() == ObjectType.JOINT: state.extend( [Joint(obj.get_handle()).get_joint_position()]) elif obj.get_type() == ObjectType.FORCE_SENSOR: forces, torques = ForceSensor(obj.get_handle()).read() state.extend(forces + torques) return np.array(state).flatten()
def __init__(self, count: int, name: str, num_joints: int, use_vision_sensor=False, base_name: str = None, max_velocity=100): self.use_vision_sensor = use_vision_sensor self.oh_joint = [ "Quadricopter_propeller_joint1", "Quadricopter_propeller_joint2", "Quadricopter_propeller_joint3", "Quadricopter_propeller_joint4" ] propeller_names = [ "Quadricopter_propeller_respondable1", "Quadricopter_propeller_respondable2", "Quadricopter_propeller_respondable3", "Quadricopter_propeller_respondable4" ] self.propellers = [Shape(name) for name in propeller_names] force_sensor_names = [ "Quadricopter_propeller1", "Quadricopter_propeller2", "Quadricopter_propeller3", "Quadricopter_propeller4" ] self.force_sensors = [ForceSensor(name) for name in force_sensor_names] super().__init__(count, name, self.oh_joint, base_name) self.joint_handles = [joint._handle for joint in self.joints] self.force_sensor_handles = [ sensor.get_handle() for sensor in self.force_sensors ] if self.use_vision_sensor: # vision_sensor_names = ["Vision_sensor_floor", 'Vision_sensor_frontal'] vision_sensor_names = ['Vision_sensor_frontal'] self.vision_sensors = [ VisionSensor(name) for name in vision_sensor_names ] # One action per joint num_act = len(self.oh_joint) # Multiple dimensions per shape #num_obs = ((len(self.oh_shape)*3*3)+1); num_obs = 18 self.joints_max_velocity = max_velocity act = np.array([self.joints_max_velocity] * num_act) obs = np.array([np.inf] * num_obs) self.action_space = spaces.Box(np.array([0] * num_act), act) self.observation_space = spaces.Box(-obs, obs)
def __init__(self, count: int, name: str, base_name: str = None): suffix = '' if count == 0 else '#%d' % (count - 1) super().__init__(name + suffix if base_name is None else base_name + suffix) self._proximity_sensor = ProximitySensor('%s_sensor%s' % (name, suffix)) self._attach_point = ForceSensor('%s_connection%s' % (name, suffix)) self._old_parents = [] self._grasped_objects = []
def init_task(self) -> None: cap_detector = ProximitySensor("cap_detector") self.joint = Joint('joint') self.force_sensor = ForceSensor('Force_sensor') self.cap = Shape('cap') self.register_success_conditions( [DetectedCondition(self.cap, cap_detector, negated=True)]) self.cap_turned_condition = JointCondition( self.joint, np.deg2rad(150))
class TestForceSensors(TestCore): def setUp(self): super().setUp() self.sensor = ForceSensor('force_sensor') def test_read(self): force_vector, torque_vector = self.sensor.read() self.assertEqual(len(force_vector), 3) self.assertEqual(len(torque_vector), 3)
def __init__(self, count: int, name: str, joint_names: List[str]): super().__init__(count, name, joint_names) suffix = '' if count == 0 else '#%d' % (count - 1) prox_name = '%s_attachProxSensor%s' % (name, suffix) attach_name = '%s_attachPoint%s' % (name, suffix) self._proximity_sensor = ProximitySensor(prox_name) self._attach_point = ForceSensor(attach_name) self._old_parents: List[Object] = [] self._grasped_objects: List[Object] = [] self._prev_positions = [None] * len(joint_names) self._prev_vels = [None] * len(joint_names) # Used to stop oscillating self._touch_sensors = [] i = 0 while True: fname = '%s_touchSensor%d%s' % (name, i, suffix) if not ForceSensor.exists(fname): break self._touch_sensors.append(ForceSensor(fname)) i += 1
def __init__(self, count: int, name: str, joint_names: List[str]): super().__init__(count, name, joint_names) suffix = '' if count == 0 else '#%d' % (count - 1) prox_name = '%s_attachProxSensor%s' % (name, suffix) attach_name = '%s_attachPoint%s' % (name, suffix) self._proximity_sensor = ProximitySensor(prox_name) self._attach_point = ForceSensor(attach_name) self._old_parents = [] self._grasped_objects = [] self._prev_positions = [None] * len(joint_names) self._prev_vels = [None] * len(joint_names) # Used to stop oscillating
def init_task(self) -> None: bottle_detector = ProximitySensor("bottle_detector") cap_detector = ProximitySensor("cap_detector") bottle = Shape('bottle') self.joint = Joint('joint') self.force_sensor = ForceSensor('Force_sensor') self.cap = Shape('cap') self.register_success_conditions( [DetectedCondition(bottle, bottle_detector), DetectedCondition(self.cap, cap_detector, negated=True), NothingGrasped(self.robot.gripper)]) self.cap_turned_condition = JointCondition( self.joint, np.deg2rad(150))
def get_low_dim_state(self) -> np.ndarray: """Gets the pose and various other properties of objects in the task. :return: 1D array of low-dimensional task state. """ objs = self.get_base().get_objects_in_tree(exclude_base=True, first_generation_only=False) state = [] for obj in objs: state.extend(np.array(obj.get_pose())) if obj.get_type() == ObjectType.JOINT: state.extend([Joint(obj.get_handle()).get_joint_position()]) elif obj.get_type() == ObjectType.FORCE_SENSOR: forces, torques = ForceSensor(obj.get_handle()).read() state.extend(forces + torques) return np.array(state).flatten()
def __init__(self, count: int, num_propeller: int, name: str): force_sensor_names = [ '%s_propeller%s' % (name, str(i + 1)) for i in range(num_propeller) ] respondable_names = [ '%s_propeller_respondable%s' % (name, str(i + 1)) for i in range(num_propeller) ] suffix = '' if count == 0 else '#%d' % (count - 1) #get the handle of the drone base super().__init__(name + suffix) self._num_propeller = num_propeller #get handles of force sensor self.force_sensors = [ ForceSensor(fname + suffix).get_handle() for fname in force_sensor_names ] #get the handles of propeller respondable self.respondables = [ Shape(sname + suffix)._handle for sname in respondable_names ] #add some simulation parameters self.particleCountPerSecond = 430 self.particleDensity = 8500 self.particleSize = 1 self.notFullParticles = 0 self.pre_v = 0 # previous size factor self.particlesTargetVelocities = [0, 0, 0, 0] self.pParam = 2 self.iParam = 0 self.dParam = 0 self.vParam = -2 self.cumul = 0 self.lastE = 0 self.pAlphaE = 0 self.pBetaE = 0 self.psp2 = 0 self.psp1 = 0 self.prevEuler = 0
class Accelerometer(Object): """An object able to measure accelerations that are applied to it. """ def __init__(self, name): super().__init__(name) self._mass_object = Shape('%s_mass' % (self.get_name())) self._sensor = ForceSensor('%s_force_sensor' % (self.get_name())) def _get_requested_type(self) -> ObjectType: return ObjectType(sim.simGetObjectType(self.get_handle())) def read(self) -> List[float]: """Reads the acceleration applied to accelerometer. :return: A list containing applied accelerations along the sensor's x, y and z-axes """ forces, _ = self._sensor.read() accel = [force / self._mass_object.get_mass() for force in forces] return accel
def setUp(self): super().setUp() self.sensor = ForceSensor('force_sensor')
def test_create(self): sensor = ForceSensor.create() sensor.remove()
def test_get_force_sensor(self): cube = ForceSensor('force_sensor') self.assertIsInstance(cube, ForceSensor)
def __init__(self, name): super().__init__(name) self._mass_object = Shape('%s_mass' % (self.get_name())) self._sensor = ForceSensor('%s_force_sensor' % (self.get_name()))