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()))