def diff_drive(self, robot, index, target_pose):
"""
this diff drive is very hacky
it tries to transport the target pose to match an end pose
by computing the pose difference between current pose and target pose
then it estimates a cartesian velocity for the end effector to follow.
It uses differential IK to compute the required joint velocity, and set
the joint velocity as current step target velocity.
This technique makes the trajectory very unstable but it still works some times.
"""
pf = robot.get_compute_functions()['passive_force'](True, True, False)
max_v = 0.1
max_w = np.pi
qpos, qvel, poses = robot.get_observation()
current_pose: Pose = poses[index]
delta_p = target_pose.p - current_pose.p
delta_q = qmult(target_pose.q, qinverse(current_pose.q))
axis, theta = quat2axangle(delta_q)
if (theta > np.pi):
theta -= np.pi * 2
t1 = np.linalg.norm(delta_p) / max_v
t2 = theta / max_w
t = max(np.abs(t1), np.abs(t2), 0.001)
thres = 0.1
if t < thres:
k = (np.exp(thres) - 1) / thres
t = np.log(k * t + 1)
v = delta_p / t
w = theta / t * axis
target_qvel = robot.get_compute_functions()['cartesian_diff_ik'](
np.concatenate((v, w)), 9)
robot.set_action(qpos, target_qvel, pf)
def get_yaw(self):
o = self.client.getOrientation()
vec, theta = quaternions.quat2axangle(
[o.w_val, o.x_val, o.y_val, o.z_val])
new_vec = self.units.vel3d_from_as(vec)
roll, pitch, yaw = euler.axangle2euler(new_vec, theta)
return yaw
def diff_drive2(self, robot, index, target_pose, js1, joint_target, js2):
"""
This is a hackier version of the diff_drive
It uses specified joints to achieve the target pose of the end effector
while asking some other specified joints to match a global pose
"""
pf = robot.get_compute_functions()['passive_force'](True, True, False)
max_v = 0.1
max_w = np.pi
qpos, qvel, poses = robot.get_observation()
current_pose: Pose = poses[index]
delta_p = target_pose.p - current_pose.p
delta_q = qmult(target_pose.q, qinverse(current_pose.q))
axis, theta = quat2axangle(delta_q)
if (theta > np.pi):
theta -= np.pi * 2
t1 = np.linalg.norm(delta_p) / max_v
t2 = theta / max_w
t = max(np.abs(t1), np.abs(t2), 0.001)
thres = 0.1
if t < thres:
k = (np.exp(thres) - 1) / thres
t = np.log(k * t + 1)
v = delta_p / t
w = theta / t * axis
target_qvel = robot.get_compute_functions()['cartesian_diff_ik'](
np.concatenate((v, w)), 9)
for j, target in zip(js2, joint_target):
qpos[j] = target
robot.set_action(qpos, target_qvel, pf)
def test_axis_angle_from_quaternion2(self):
q = [0,0,0,1.0000001]
axis2, angle2 = quat2axangle([q[-1],q[0],q[1],q[2]])
x,y,z, angle = speed_up_and_execute(spw.axis_angle_from_quaternion, list(q))
axis = [x,y,z]
angle = float(angle)
compare_axis_angle(angle, axis, angle2, axis2, 2)
def euler2axangle(z=0, y=0, x=0):
''' Return axis, angle corresponding to these Euler angles
Uses the z, then y, then x convention above
Parameters
----------
z : scalar
Rotation angle in radians around z-axis (performed first)
y : scalar
Rotation angle in radians around y-axis
x : scalar
Rotation angle in radians around x-axis (performed last)
Returns
-------
vector : array shape (3,)
axis around which rotation occurs
theta : scalar
angle of rotation
Examples
--------
>>> vec, theta = euler2axangle(0, 1.5, 0)
>>> np.allclose(vec, [0, 1, 0])
True
>>> theta
1.5
'''
# delayed import to avoid cyclic dependencies
import transforms3d.quaternions as nq
return nq.quat2axangle(euler2quat(z, y, x))
def test_quat2axangle():
ax, angle = tq.quat2axangle([1, 0, 0, 0])
assert_array_equal(ax, [1, 0, 0])
assert_array_equal(angle, 0)
# Non-normalized quaternion, unit quaternion
ax, angle = tq.quat2axangle([5, 0, 0, 0])
assert_array_equal(ax, [1, 0, 0])
assert_array_equal(angle, 0)
# Rotation by 90 degrees around x
r2d2 = np.sqrt(2) / 2.
quat_x_90 = np.array([r2d2, r2d2, 0, 0])
ax, angle = tq.quat2axangle(quat_x_90)
assert_almost_equal(ax, [1, 0, 0])
assert_almost_equal(angle, np.pi / 2)
# Not-normalized version of same, gives same output
ax, angle = tq.quat2axangle(quat_x_90 * 7)
assert_almost_equal(ax, [1, 0, 0])
assert_almost_equal(angle, np.pi / 2)
# Any non-finite value gives nan angle
for pos in range(4):
for val in np.nan, np.inf, -np.inf:
q = [1, 0, 0, 0]
q[pos] = val
ax, angle = tq.quat2axangle(q)
assert_almost_equal(ax, [1, 0, 0])
assert np.isnan(angle)
# Infinite length likewise, because of length overflow
f64info = np.finfo(np.float64)
ax, angle = tq.quat2axangle([2, f64info.max, 0, 0])
assert_almost_equal(ax, [1, 0, 0])
assert np.isnan(angle)
# Very small values give indentity transformation
ax, angle = tq.quat2axangle([0, f64info.eps / 2, 0, 0])
assert_almost_equal(ax, [1, 0, 0])
assert angle == 0
def test_quat2axangle():
ax, angle = tq.quat2axangle([1, 0, 0, 0])
assert_array_equal(ax, [1, 0, 0])
assert_array_equal(angle, 0)
# Non-normalized quaternion, unit quaternion
ax, angle = tq.quat2axangle([5, 0, 0, 0])
assert_array_equal(ax, [1, 0, 0])
assert_array_equal(angle, 0)
# Rotation by 90 degrees around x
r2d2 = np.sqrt(2) / 2.
quat_x_90 = np.array([r2d2, r2d2, 0, 0])
ax, angle = tq.quat2axangle(quat_x_90)
assert_almost_equal(ax, [1, 0, 0])
assert_almost_equal(angle, np.pi / 2)
# Not-normalized version of same, gives same output
ax, angle = tq.quat2axangle(quat_x_90 * 7)
assert_almost_equal(ax, [1, 0, 0])
assert_almost_equal(angle, np.pi / 2)
# Any non-finite value gives nan angle
for pos in range(4):
for val in np.nan, np.inf, -np.inf:
q = [1, 0, 0, 0]
q[pos] = val
ax, angle = tq.quat2axangle(q)
assert_almost_equal(ax, [1, 0, 0])
assert np.isnan(angle)
# Infinite length likewise, because of length overflow
f64info = np.finfo(np.float64)
ax, angle = tq.quat2axangle([2, f64info.max, 0, 0])
assert_almost_equal(ax, [1, 0, 0])
assert np.isnan(angle)
# Very small values give indentity transformation
ax, angle = tq.quat2axangle([0, f64info.eps / 2, 0, 0])
assert_almost_equal(ax, [1, 0, 0])
assert angle == 0
def test_axis_angle_from_quaternion(self, q):
axis2, angle2 = quat2axangle([q[-1], q[0], q[1], q[2]])
axis = w.compile_and_execute(
lambda x, y, z, w_: w.axis_angle_from_quaternion(x, y, z, w_)[0],
q)
angle = w.compile_and_execute(
lambda x, y, z, w_: w.axis_angle_from_quaternion(x, y, z, w_)[1],
q)
compare_axis_angle(angle, axis, angle2, axis2, 2)
def test_axis_angle():
for M, q in eg_pairs:
vec, theta = tq.quat2axangle(q)
q2 = tq.axangle2quat(vec, theta)
assert tq.nearly_equivalent(q, q2)
aa_mat = taa.axangle2mat(vec, theta)
assert_array_almost_equal(aa_mat, M)
aa_mat2 = sympy_aa2mat(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat2)
aa_mat22 = sympy_aa2mat2(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat22)
def _interpolate_transformation(t_1, t_2, coefficient):
assert 0 <= coefficient < 1
t_result = np.identity(4)
t_result[0:3, 3] = t_1[0:3, 3] * (1 - coefficient) + t_2[0:3, 3] * coefficient
rot_diff = t_2[0:3, 0:3].dot(t_1[0:3, 0:3].transpose())
q_diff = quaternions.mat2quat(rot_diff)
axis, angle = quaternions.quat2axangle(q_diff)
rot_delta = quaternions.quat2mat(quaternions.axangle2quat(axis, angle * coefficient))
rot_result = rot_delta.dot(t_1[0:3, 0:3])
t_result[0:3, 0:3] = rot_result
return t_result
def test_axis_angle():
for M, q in eg_pairs:
vec, theta = tq.quat2axangle(q)
q2 = tq.axangle2quat(vec, theta)
assert tq.nearly_equivalent(q, q2)
aa_mat = taa.axangle2mat(vec, theta)
assert_array_almost_equal(aa_mat, M)
aa_mat2 = sympy_aa2mat(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat2)
aa_mat22 = sympy_aa2mat2(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat22)
def rotation(value, r):
q0 = quaternions.axangle2quat(
[float(value[0]), float(value[1]),
float(value[2])], float(value[3]))
q1 = quaternions.axangle2quat([r[0], r[1], r[2]], r[3])
qr = quaternions.qmult(q1, q0)
v, theta = quaternions.quat2axangle(qr)
return [
WebotsParser.str(v[0]),
WebotsParser.str(v[1]),
WebotsParser.str(v[2]),
WebotsParser.str(theta)
]
def rot_diff(self, other):
'''Compute rotation btw frames
Arguments:
other {Frame} -- target frame
Returns:
np.array, float -- axis and angle
'''
dq = quaternions.qmult(other.q, quaternions.qinverse(self.q))
axis, angle = quaternions.quat2axangle(dq)
return axis, angle
def test_angle_axis_imps():
for x, y, z in euler_tuples:
M = ttb.euler2mat(z, y, x)
q = tq.mat2quat(M)
vec, theta = tq.quat2axangle(q)
M1 = axangle2mat(vec, theta)
M2 = axangle2aff(vec, theta)[:3,:3]
assert_array_almost_equal(M1, M2)
v1, t1 = mat2axangle(M1)
M3 = axangle2mat(v1, t1)
assert_array_almost_equal(M, M3)
A = np.eye(4)
A[:3,:3] = M
v2, t2, point = aff2axangle(A)
M4 = axangle2mat(v2, t2)
assert_array_almost_equal(M, M4)
def rotation_mat2vec(rotation):
""" Rotation vector from rotation matrix.
Parameters
----------
rotation: array (3, 3)
rotation matrix.
Returns
-------
vec: array (3, )
rotation vector, where norm of vec is the angle theta, and the
axis of rotation is given by vec / theta.
"""
ax, angle = quat2axangle(mat2quat(rotation))
return ax * angle
def diff_drive(self,
robot: PandaArm,
index,
target_pose,
max_v=0.1,
max_w=np.pi,
active_joints_ids=[],
override=None):
obs = robot.observation
qpos, qvel, poses = obs['qpos'], obs['qvel'], obs['poses']
current_pose: Pose = poses[index]
delta_p = target_pose.p - current_pose.p
delta_q = quat.qmult(target_pose.q, quat.qinverse(current_pose.q))
axis, theta = quat.quat2axangle(delta_q)
if theta > np.pi:
theta -= np.pi * 2
t1 = np.linalg.norm(delta_p) / max_v
t2 = theta / max_w
t = max(np.abs(t1), np.abs(t2), 0.01)
thres = 0.1
if t < thres:
k = (np.exp(thres) - 1) / thres
t = np.log(k * t + 1)
v = delta_p / t
w = theta / t * axis
index = index if index >= 0 else len(robot._robot.get_joints()) + index
cal_qvel = robot.get_compute_functions()['cartesian_diff_ik'](
np.concatenate((v, w)), index, active_joints_ids)
target_qvel = np.zeros_like(qvel)
indx = np.arange(
len(qvel)) if active_joints_ids == [] else active_joints_ids
target_qvel[indx] = cal_qvel
pf = robot.get_compute_functions()['passive_force']()
if override is not None:
override_indx, override_qpos, override_qvel = override
qpos[override_indx] = override_qpos
target_qvel[override_indx] = override_qvel
robot.set_action(qpos, target_qvel, pf)
def quar_axis_error(q_sp, q_state):
"""Compute the error in quaternions from the setpoints and robot state
Args:
q_sp (np.array): Reference signal Set point quaternion
q_state (np.array): Sensor Feedback quaternion
Returns:
exponential angle (np.array)
"""
# Quaternion multiplication q_set * (q_state)' target - state
q_state_conj = quaternions.qconjugate(q_state)
q_error = quaternions.qmult(q_sp, q_state_conj)
# Nearest rotation
if (q_error[0] < 0):
q_error = -1. * q_error
axis_error = quaternions.quat2axangle(q_error)
return axis_error[0] * axis_error[1]
def angular_distance(quat):
vec, theta = quat2axangle(quat)
return theta / np.pi * 180
def quatToAxisAngle(q):
[vec, theta] = quat.quat2axangle(q)
return theta * vec
def generate_one_im_anno(cnt, cam_name, peg_top, peg_bottom, hole_top, hole_bottom, rob_arm, im_data_path, delta_rotation_quat, delta_translation):
# Get keypoint location
peg_keypoint_top_pose = rob_arm.get_object_matrix(peg_top)
peg_keypoint_bottom_pose = rob_arm.get_object_matrix(peg_bottom)
hole_keypoint_top_pose = rob_arm.get_object_matrix(hole_top)
hole_keypoint_bottom_pose = rob_arm.get_object_matrix(hole_bottom)
peg_keypoint_top_in_world = peg_keypoint_top_pose[:3,3]
peg_keypoint_bottom_in_world = peg_keypoint_bottom_pose[:3, 3]
hole_keypoint_top_in_world = hole_keypoint_top_pose[:3,3]
hole_keypoint_bottom_in_world = hole_keypoint_bottom_pose[:3, 3]
# Get RGB images from camera
rgb = rob_arm.get_rgb(cam_name=cam_name)
# rotate rgb 180
(h, w) = rgb.shape[:2]
center = (w / 2, h / 2)
M = cv2.getRotationMatrix2D(center, 180, 1.0)
rgb = cv2.warpAffine(rgb, M, (w, h))
# Get depth from camera
depth = rob_arm.get_depth(cam_name=cam_name, near_plane=0.01, far_plane=1.5)
# rotate depth 180
(h, w) = depth.shape[:2]
center = (w / 2, h / 2)
M = cv2.getRotationMatrix2D(center, 180, 1.0)
depth = cv2.warpAffine(depth, M, (w, h))
depth_mm = (depth*1000).astype(np.uint16) # type: np.uint16 ; uint16 is needed by keypoint detection network
#rob_arm.visualize_image(rgb=rgb, depth=depth)
# Save image
rgb_file_name = str(cnt).zfill(6)+'_rgb.png'
rgb_file_path = os.path.join(im_data_path, rgb_file_name)
depth_file_name = str(cnt).zfill(6)+'_depth.png'
depth_file_path = os.path.join(im_data_path, depth_file_name)
cv2.imwrite(rgb_file_path, rgb)
cv2.imwrite(depth_file_path, depth_mm)
# Get camera info
cam_pos, cam_quat, cam_matrix= rob_arm.get_camera_pos(cam_name=cam_name)
camera_to_world = {'quaternion':{'x':cam_quat[0], 'y':cam_quat[1], 'z':cam_quat[2], 'w':cam_quat[3]},
'translation':{'x':cam_pos[0], 'y':cam_pos[1], 'z':cam_pos[2]}}
camera2world_map = camera_to_world
world2camera = world2camera_from_map(camera2world_map)
peg_keypoint_top_in_world = np.append(peg_keypoint_top_in_world,[1],axis = 0).reshape(4,1)
peg_keypoint_bottom_in_world = np.append(peg_keypoint_bottom_in_world, [1], axis=0).reshape(4, 1)
hole_keypoint_top_in_world = np.append(hole_keypoint_top_in_world,[1],axis = 0).reshape(4,1)
hole_keypoint_bottom_in_world = np.append(hole_keypoint_bottom_in_world,[1],axis = 0).reshape(4,1)
peg_keypoint_top_in_camera = world2camera.dot(peg_keypoint_top_in_world)
peg_keypoint_bottom_in_camera = world2camera.dot(peg_keypoint_bottom_in_world)
hole_keypoint_top_in_camera = world2camera.dot(hole_keypoint_top_in_world)
hole_keypoint_bottom_in_camera = world2camera.dot(hole_keypoint_bottom_in_world)
peg_keypoint_top_in_camera = peg_keypoint_top_in_camera[:3].reshape(3,)
peg_keypoint_bottom_in_camera = peg_keypoint_bottom_in_camera[:3].reshape(3, )
hole_keypoint_top_in_camera = hole_keypoint_top_in_camera[:3].reshape(3,)
hole_keypoint_bottom_in_camera = hole_keypoint_bottom_in_camera[:3].reshape(3, )
keypoint_in_camera = np.array([peg_keypoint_top_in_camera, peg_keypoint_bottom_in_camera, hole_keypoint_top_in_camera, hole_keypoint_bottom_in_camera])
#keypoint_in_camera = np.array([peg_keypoint_bottom_in_camera, hole_keypoint_top_in_camera])
xy_depth = point2pixel(keypoint_in_camera)
xy_depth_list = []
(h, w) = rgb.shape[:2]
for i in range(len(xy_depth)):
x = int(xy_depth[i][0])
y = int(xy_depth[i][1])
z = int(xy_depth[i][2])
xy_depth_list.append([x,y,z])
rot_vec, rot_theta = quat2axangle(delta_rotation_quat)
delta_rotation_matrix = quat2mat(delta_rotation_quat)
info = {'3d_keypoint_camera_frame': [[float(v) for v in peg_keypoint_top_in_camera], [float(v) for v in peg_keypoint_bottom_in_camera], [float(v) for v in hole_keypoint_top_in_camera], [float(v) for v in hole_keypoint_bottom_in_camera]],
#'3d_keypoint_camera_frame':[ [float(v) for v in peg_keypoint_bottom_in_camera],
# [float(v) for v in hole_keypoint_top_in_camera]],
'bbox_bottom_right_xy': [rgb.shape[1], rgb.shape[0]],
'bbox_top_left_xy': [0,0],
'camera_to_world': camera_to_world,
'depth_image_filename': depth_file_name,
'rgb_image_filename': rgb_file_name,
'keypoint_pixel_xy_depth': xy_depth_list,
'delta_rotation_quat': delta_rotation_quat.tolist(),
'delta_rotation_matrix': delta_rotation_matrix.tolist(),
'delta_translation': delta_translation.tolist(),
'delta_rot_vec': rot_vec.tolist(),
'delta_rot_theta': rot_theta
}
return info