def _set_markers(self, forces, tips, cube_force, cube_pos):
R = 0.005
L = 0.2
ms = []
cube_force = cube_force / np.linalg.norm(cube_force)
q = get_rotation_between_vecs(np.array([0, 0, 1]), cube_force)
if not self.markers:
ms.append(
CylinderMarker(R,
L,
cube_pos + 0.5 * L * cube_force,
q,
color=(0, 0, 1, 0.5)))
else:
self.markers[0].set_state(cube_pos + 0.5 * L * cube_force, q)
ms.append(self.markers[0])
for i, (f, t) in enumerate(zip(forces, tips)):
f = f / np.linalg.norm(f)
q = get_rotation_between_vecs(np.array([0, 0, 1]), f)
if not self.markers:
ms.append(
CylinderMarker(R,
L,
t + 0.5 * L * f,
q,
color=(1, 0, 0, 0.5)))
else:
self.markers[i + 1].set_state(t + 0.5 * L * f, q)
ms.append(self.markers[i + 1])
self.markers = ms
def add_tip_adjustments(self, obs, num_steps=50):
print("Appending to path....")
# tip_pos = self.path.tip_path[-1]
dir = 0.5 * (obs['goal_object_position'] - obs['object_position'])
cube_pos = self.cube_sequence[-1][:3]
cube_ori = p.getQuaternionFromEuler(self.cube_sequence[-1][3:])
grasp = self.path.grasp
if self.adjust_tip_ori:
yaxis = np.array([0, 1, 0])
goal_obj_yaxis = Rotation.from_quat(
obs['goal_object_orientation']).apply(yaxis)
obj_yaxis = Rotation.from_quat(cube_ori).apply(yaxis)
diff_quat = get_rotation_between_vecs(obj_yaxis, goal_obj_yaxis)
resolution = np.arange(0, 1, 1.0 / num_steps)
interp_quat = slerp(np.array([0, 0, 0, 1]), diff_quat, resolution)
warning_counter = 0
warning_tips = []
for i in range(num_steps):
translation = cube_pos + i / num_steps * dir
if self.adjust_tip_ori:
rotation = (Rotation.from_quat(interp_quat[i]) *
Rotation.from_quat(cube_ori)).as_quat()
else:
rotation = cube_ori
goal_tip_pos = Transform(translation, rotation)(grasp.cube_tip_pos)
q = obs['robot_position']
for j, tip in enumerate(goal_tip_pos):
q = self.env.pinocchio_utils.inverse_kinematics(j, tip, q)
if q is None:
q = self.joint_sequence[-1]
# print(f'[tip adjustments] warning: IK solution is not found for tip {j}. Using the last joint conf')
warning_counter += 1
if j not in warning_tips:
warning_tips.append(j)
break
if q is None:
print(
'[tip adjustments] warning: IK solution is not found for all tip positions.'
)
print(
f'[tip adjustments] aborting tip adjustments (loop {i} / {num_steps})'
)
break
target_cube_pose = np.concatenate(
[translation, p.getEulerFromQuaternion(rotation)])
self.cube_sequence.append(target_cube_pose)
self.joint_sequence.append(q)
self.path.tip_path.append(goal_tip_pos)
if warning_counter > 0:
print(
f'[tip adjustments] warning: IK solution is not found for {warning_counter} / {num_steps} times on tips {warning_tips}.'
)
def _get_cone_corners(self):
# approximate cone with an inscribed square
contact_normal = np.array([0, 0, 1])
fac = self.mu * np.sqrt(2) / 2
corners = []
transforms = []
for i in [-1, +1]:
for j in [-1, +1]:
corner = np.array([i * fac, j * fac, 1])
corner /= np.linalg.norm(corner)
q = get_rotation_between_vecs(contact_normal, corner)
corners.append(corner)
transforms.append(Transform(pos=np.zeros(3), ori=q))
return corners, transforms
def get_closest_pitch_angle(self):
"""The competition_reward doesn't care about pitch rotation,
so we should use the smallest rotation that matches the roll and yaw
of the goal.
This function returns the goal orientation with that criterion.
"""
actual_rot = Rotation.from_quat(self.obs['object_orientation'])
goal_rot = Rotation.from_quat(self.obs['goal_object_orientation'])
y_axis = [0, 1, 0]
goal_direction_vector = goal_rot.apply(y_axis)
actual_direction_vector = actual_rot.apply(y_axis)
quat = get_rotation_between_vecs(actual_direction_vector,
goal_direction_vector)
return (Rotation.from_quat(quat) * actual_rot).as_quat()
def contact_from_tip_position(self, pos):
"""
Compute contact frame from tip positions in the cube
center of mass frame.
"""
outside = np.abs(pos) >= self.halfsize - 1e-5
sign = np.sign(pos)
contact = pos.copy()
normal = np.zeros(3)
if np.sum(outside) == 0:
outside[:] = True
dist = np.minimum(np.abs(pos - self.halfsize),
np.abs(pos + self.halfsize))
axes = np.argsort(dist)
ax = [ax for ax in axes if outside[ax]][0]
contact[ax] = sign[ax] * self.halfsize[ax]
normal[ax] = -sign[ax]
q = get_rotation_between_vecs(np.array([0, 0, 1]), normal)
return Transform(pos=contact, ori=q)