def getV(self):
"""
Get the associated view matrix.
"""
roll, pitch, yaw = get_rpy_from_quaternion(self.orientation)
self.up_vector = (0, 0, 1)
up_axis_index = 2 # z axis
# use the specified distance to calculate it.
target_position = self.position + self.distance * np.array([
-np.cos(pitch) * np.cos(-yaw), -np.cos(pitch) * np.sin(-yaw),
-np.sin(pitch)
])
# compute view matrix
self._V = self.sim.compute_view_matrix(eye_position=self.position,
target_position=target_position,
up_vector=self.up_vector)
# self._V = self.sim.compute_view_matrixFromYawPitchRoll(cameraTargetPosition=target_position,
# distance=distance,
# yaw=yaw,
# pitch=pitch,
# roll=roll,
# upAxisIndex=up_axis_index)
return self._V
def step(self, update_interface=False):
"""Perform a step: map the mouse-keyboard interface to the world"""
# update interface
if update_interface:
self.interface()
# check keyboard events
self.check_key_events()
# set camera view
pitch, yaw = get_rpy_from_quaternion(self.robot.orientation)[1:]
if self.fpv: # first-person view
target_pos = self.robot.position + 2 * np.array([
np.cos(yaw) * np.cos(pitch),
np.sin(yaw) * np.cos(pitch),
np.sin(pitch)
])
self.camera.reset(distance=2,
pitch=-pitch,
yaw=yaw - np.pi / 2,
target_position=target_pos)
else: # third-person view
self.camera.follow(body_id=self.robot.id,
distance=2,
yaw=yaw - np.pi / 2,
pitch=self.camera_pitch)
def step(self, update_interface=False):
"""Perform a step: map the SpaceMouse interface to the end-effector."""
# update interface
if update_interface:
self.interface()
# update the QP task desired position
position = self.manipulator.get_link_positions(
link_ids=self.distal_link, wrt_link_id=self.base_link)
position += self.interface.translation
self.task.desired_position = position
# update the QP task desired orientation (if specified)
if self.use_orientation:
orientation = self.manipulator.get_link_orientations(
link_ids=self.distal_link, wrt_link_id=self.base_link)
orientation = get_rpy_from_quaternion(orientation)
orientation += self.interface.rotation
self.task.desired_orientation = get_quaternion_from_rpy(
orientation)
# update the QP task
self.task.update(update_model=True)
# solve QP and set the joint variables
if self._control_type == ControlType.POSITION: # Position control
# solve QP task
dq = self.solver.solve()
# set joint positions
q = self.manipulator.get_joint_positions()
q = q + dq * self.simulator.dt
self.manipulator.set_joint_positions(q)
else: # Impedance control
# solve QP task
torques = self.solver.solve()
# set joint torques
self.manipulator.set_joint_torques(torques)
# check if gripper
if self.gripper is not None:
# check if button is pressed
if self.interface.left_button_pressed: # open the gripper
if self._control_type == ControlType.POSITION: # position control
self.gripper.open(factor=1)
else: # impedance control
self.grasp_strength += 1
self.gripper.grasp(strength=self.grasp_strength)
elif self.interface.right_button_pressed: # close the gripper
if self._control_type == ControlType.POSITION: # position control
self.gripper.close(factor=1)
else: # impedance control
self.grasp_strength -= 1
self.gripper.grasp(strength=self.grasp_strength)
def _write_continuous(self, data):
"""apply the action data on the robot."""
# get current orientations and convert them to RPY angles
orientation = self.robot.get_link_world_orientations(link_ids=self.link) # (4,)
orientation = get_rpy_from_quaternion(orientation) # (3,)
# add change in orientations
data += orientation # (3,)
# convert them back to quaternions
data = get_quaternion_from_rpy(data) # (4,)
self.robot.set_link_positions(link_ids=self.link, orientations=data)
def _get_states(self):
"""Return the link orientation state."""
# get the link orientation
if self.wrt_link is None:
orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=True)
else:
orientation = self.robot.get_link_orientations(link_ids=self.link, wrt_link_id=self.wrt_link, flatten=True)
# convert from quaternion to roll-pitch-yaw angles
orientation = get_rpy_from_quaternion(orientation)
# return the proper orientation
if self.dim is None:
return orientation
return orientation[self.dim]
def _write_continuous(self, data):
"""apply the action data on the robot."""
# get current pose
position = self.robot.get_link_world_poses(link_ids=self.link, flatten=False) # (3,)
orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=False) # (4,)
orientation = get_rpy_from_quaternion(orientation) # (3,)
# add changes
data[:3] += position # (3,)
data[3:] += orientation # (3,)
# convert back orientations to quaternions
data = np.concatenate((data[:3], get_quaternion_from_rpy(data[3:]))) # (7,)
# write poses
self.robot.set_link_positions(link_ids=self.link, positions=data[:3], orientations=data[3:])
def orientation(self, orientation):
"""
Set the orientation (expressed as a quaternion, rotation matrix, or roll-pitch-yaw angles) of the camera.
"""
# convert the orientation to roll-pitch-yaw angle
if orientation.shape == (4, ): # quaternion (x,y,z,w)
rpy = get_rpy_from_quaternion(orientation)
elif orientation.shape == (3, 3): # rotation matrix
rpy = get_rpy_from_matrix(orientation)
elif orientation.shape == (3, ): # roll-pitch-yaw angle
rpy = orientation
else:
raise ValueError(
"Expecting the given orientation to be a quaternion, rotation matrix, or Roll-Pitch-Yaw "
"angles, instead got: {}".format(orientation))
# reset the camera
distance, target_position = self.sim.get_debug_visualizer()[-2:]
_, pitch, yaw = rpy
self.reset(distance, yaw, pitch, target_position)
def _get_states(self):
"""Return the state."""
orientation = get_rpy_from_quaternion(self.body.orientation)
if self.dim is None:
return orientation
return orientation[self.dim]
def rpy(self):
"""Return the orientation as the Roll-Pitch-Yaw angles."""
return get_rpy_from_quaternion(self.orientation)
# create robot
scale = 1. # Warning: this does not scale the mass...
position = [0., 0., np.sqrt(2) / 2. * scale + 0.001]
orientation = [0.383, 0, 0, 0.924]
robot = Cubli(sim, position, orientation, scale=scale)
# print information about the robot
robot.print_info()
H = robot.get_mass_matrix(q_idx=slice(
6, 6 + len(robot.joints))) # floating base, thus keep only the last q
print("Inertia matrix: H(q) = {}\n".format(H))
# PD control
Kp = 600.
Kd = 2 * np.sqrt(Kp)
desired_roll = np.pi / 4.
for i in count():
# get state
quaternion = robot.get_base_orientation()
w = robot.get_base_angular_velocity()
euler = get_rpy_from_quaternion(quaternion)
# PD control
torques = [-Kp * (desired_roll - euler[0]) + Kd * w[0], 0., 0.]
robot.set_joint_torques(torques)
# step in simulation
world.step(sleep_dt=1. / 240)
def step(self, update_interface=False):
"""Perform a step: map the Controller interface to the end-effector."""
# update interface
if update_interface:
self.interface()
# update the QP task desired position
left_joystick = self.interface.LJ[::-1] # (y,x)
right_joystick = self.interface.RJ[::-1] # (y,x)
translation = np.zeros(3)
translation[:2] = left_joystick
translation[2] = right_joystick[0]
position = self.manipulator.get_link_positions(
link_ids=self.distal_link, wrt_link_id=self.base_link)
position += self.translation_scale * self.interface.translation
self.task.desired_position = position
# update the QP task desired orientation (if specified)
if self.use_orientation:
drpy = np.zeros(3)
if self.interface.BTN_TL:
drpy[0] = self.rotation_step
elif self.interface.BTN_TR:
drpy[0] = -self.rotation_step
if self.interface.BTN_TL2:
drpy[1] = self.rotation_step
elif self.interface.BTN_TR2:
drpy[1] = -self.rotation_step
if self.interface.BTN_WEST:
drpy[2] = self.rotation_step
elif self.interface.BTN_EAST:
drpy[2] = -self.rotation_step
orientation = self.manipulator.get_link_orientations(
link_ids=self.distal_link, wrt_link_id=self.base_link)
orientation = get_rpy_from_quaternion(orientation)
orientation += drpy
self.task.desired_orientation = get_quaternion_from_rpy(
orientation)
# update the QP task
self.task.update(update_model=True)
# solve QP and set the joint variables
if self._control_type == ControlType.POSITION: # Position control
# solve QP task
dq = self.solver.solve()
# set joint positions
q = self.manipulator.get_joint_positions()
q = q + dq * self.simulator.dt
self.manipulator.set_joint_positions(q)
else: # Impedance control
# solve QP task
torques = self.solver.solve()
# set joint torques
self.manipulator.set_joint_torques(torques)
# check if gripper
if self.gripper is not None:
# check if button is pressed
if self.interface.BTN_NORTH: # open the gripper
if self._control_type == ControlType.POSITION: # position control
self.gripper.open(factor=1)
else: # impedance control
self.grasp_strength += 0.1
self.gripper.grasp(strength=self.grasp_strength)
elif self.interface.BTN_SOUTH: # close the gripper
if self._control_type == ControlType.POSITION: # position control
self.gripper.close(factor=1)
else: # impedance control
self.grasp_strength -= 0.1
self.gripper.grasp(strength=self.grasp_strength)
def create_primitive_object(self,
shape_type,
position,
mass,
orientation=(0., 0., 0., 1.),
radius=0.5,
half_extents=(.5, .5, .5),
height=1.,
filename=None,
mesh_scale=(1., 1., 1.),
plane_normal=(0., 0., 1.),
rgba_color=None,
specular_color=None,
frame_position=None,
frame_orientation=None,
vertices=None,
indices=None,
uvs=None,
normals=None,
flags=-1):
"""Create a primitive object in the simulator. This is basically the combination of `create_visual_shape`,
`create_collision_shape`, and `create_body`.
Args:
shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4),
GEOM_PLANE (=6), GEOM_MESH (=5)
position (np.array[float[3]]): Cartesian world position of the base
mass (float): mass of the base, in kg (if using SI units)
orientation (np.array[float[4]]): Orientation of base as quaternion [x,y,z,w]
radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER
half_extents (np.array[float[3]], list/tuple of 3 floats): only for GEOM_BOX.
height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (height = length).
filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each
object (marked as 'o') in the .obj file.
mesh_scale (np.array[float[3]], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.array[float[3]], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1].
specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range
[0..1]
frame_position (np.array[float[3]]): translational offset of the visual and collision shape with respect
to the link frame.
frame_orientation (np.array[float[4]]): rotational offset (quaternion x,y,z,w) of the visual and collision
shape with respect to the link frame.
vertices (list[np.array[float[3]]]): Instead of creating a mesh from obj file, you can provide vertices,
indices, uvs and normals
indices (list[int]): triangle indices, should be a multiple of 3.
uvs (list of np.array[2]): uv texture coordinates for vertices. Use `changeVisualShape` to choose the
texture image. The number of uvs should be equal to number of vertices.
normals (list[np.array[float[3]]]): vertex normals, number should be equal to number of vertices.
flags (int): unused / to be decided
Returns:
int: non-negative unique id for primitive object, or -1 for failure
"""
# check given parameters
color = rgba_color[:3] if rgba_color is not None else None
orientation = get_rpy_from_quaternion(
orientation).tolist() if orientation is not None else None
static = True if mass == 0 else False
mass = mass if mass <= 0 else mass
position = list(position)
# shape
if shape_type == self.GEOM_BOX:
size = [
2 * half_extents[0], 2 * half_extents[1], 2 * half_extents[2]
]
shape = Shape.create(PrimitiveShape.CUBOID,
size=size,
mass=mass,
color=color,
position=position,
orientation=orientation,
static=static)
elif shape_type == self.GEOM_SPHERE:
shape = Shape.create(PrimitiveShape.SPHERE,
size=[1.],
mass=mass,
color=[1, 0, 0],
position=position,
orientation=orientation,
static=static)
elif shape_type == self.GEOM_CYLINDER:
shape = Shape.create(PrimitiveShape.CYLINDER,
size=[radius, height],
mass=mass,
color=color,
position=position,
orientation=orientation,
static=static)
elif shape_type == self.GEOM_CONE:
shape = Shape.create(PrimitiveShape.CONE,
size=[radius, height],
mass=color,
position=position,
orientation=orientation,
static=static)
# elif shape_type == self.GEOM_MESH:
# # TODO: use trimesh library
# shape = Shape.create_mesh()
else:
raise NotImplementedError(
"Primitive object not defined for the given shape type.")
# save shape and return unique id
self._body_cnt += 1
self._bodies[self._body_cnt] = shape
return self._body_cnt
def step(self, update_interface=False):
"""Perform a step: map the SpaceMouse interface to the end-effector."""
# update interface
if update_interface:
self.interface()
# get interface values
key, pressed, down = self.interface.key, self.interface.key_pressed, self.interface.key_down
# update the QP task desired position
translation = np.zeros(3)
if key.ctrl not in pressed:
if key.top_arrow in down:
translation[0] = self.translation_step
elif key.bottom_arrow in down:
translation[0] = -self.translation_step
elif key.left_arrow in down:
translation[1] = self.translation_step
elif key.right_arrow in down:
translation[1] = -self.translation_step
elif key.enter in down:
translation[2] = self.translation_step
elif key.shift in down:
translation[2] = -self.translation_step
position = self.manipulator.get_link_positions(
link_ids=self.distal_link, wrt_link_id=self.base_link)
position += translation
self.task.desired_position = position
# update the QP task desired orientation (if specified)
if self.use_orientation:
drpy = np.zeros(3)
if key.ctrl in pressed:
if key.top_arrow in down:
drpy[1] = self.rotation_step
elif key.bottom_arrow in down:
drpy[1] = -self.rotation_step
elif key.left_arrow in down:
drpy[2] = self.rotation_step
elif key.right_arrow in down:
drpy[2] = -self.rotation_step
elif key.enter in down:
drpy[0] = self.rotation_step
elif key.shift in down:
drpy[0] = -self.rotation_step
orientation = self.manipulator.get_link_orientations(
link_ids=self.distal_link, wrt_link_id=self.base_link)
orientation = get_rpy_from_quaternion(orientation)
orientation += drpy
self.task.desired_orientation = get_quaternion_from_rpy(
orientation)
# update the QP task
self.task.update(update_model=True)
# solve QP and set the joint variables
if self._control_type == ControlType.POSITION: # Position control
# solve QP task
dq = self.solver.solve()
# set joint positions
q = self.manipulator.get_joint_positions()
q = q + dq * self.simulator.dt
self.manipulator.set_joint_positions(q)
else: # Impedance control
# solve QP task
torques = self.solver.solve()
# set joint torques
self.manipulator.set_joint_torques(torques)
# check if gripper
if self.gripper is not None:
# check if button is pressed
if key.n0 in down: # open the gripper
if self._control_type == ControlType.POSITION: # position control
self.gripper.open(factor=1)
else: # impedance control
self.grasp_strength += 0.1
self.gripper.grasp(strength=self.grasp_strength)
elif key.n1 in down: # close the gripper
if self._control_type == ControlType.POSITION: # position control
self.gripper.close(factor=1)
else: # impedance control
self.grasp_strength -= 0.1
self.gripper.grasp(strength=self.grasp_strength)
