def draw_trajectory(self, pose_array, angles, ns="default_ns"):
decimation = max(len(pose_array.poses)//6, 1)
ps = gm.PoseStamped()
ps.header.frame_id = pose_array.header.frame_id
ps.header.stamp = rospy.Time.now()
handles = []
multiplier = 5.79
for (pose,angle) in zip(pose_array.poses,angles)[::decimation]:
ps.pose = deepcopy(pose)
pointing_axis = transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])[:3,0]
ps.pose.position.x -= .18*pointing_axis[0]
ps.pose.position.y -= .18*pointing_axis[1]
ps.pose.position.z -= .18*pointing_axis[2]
root_link = "r_wrist_roll_link"
valuedict = {"r_gripper_l_finger_joint":angle*multiplier,
"r_gripper_r_finger_joint":angle*multiplier,
"r_gripper_l_finger_tip_joint":angle*multiplier,
"r_gripper_r_finger_tip_joint":angle*multiplier,
"r_gripper_joint":angle}
handles.extend(self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
return handles
def params_to_mat44(trans_params, cam_cali_mat):
"""
Transform the parameters in Aurora files into 4 x 4 matrix
:param trans_params: transformation parameters in Aurora.pos. Only the last 7 are useful
3 are translations, 4 are the quaternion (x, y, z, w) for rotation
:return: 4 x 4 transformation matrix
"""
if trans_params.shape[0] == 9:
trans_params = trans_params[2:]
translation = trans_params[:3]
quaternion = trans_params[3:]
""" Transform quaternion to 3 x 3 rotation matrix, get rid of unstable scipy codes"""
# r_mat = R.from_quat(quaternion).as_matrix()
# print('r_mat\n{}'.format(r_mat))
new_quat = np.zeros((4, ))
new_quat[0] = quaternion[-1]
new_quat[1:] = quaternion[:3]
r_mat = tfms.quaternion_matrix(quaternion=new_quat)[:3, :3]
# print('my_mat\n{}'.format(r_mat))
trans_mat = np.zeros((4, 4))
trans_mat[:3, :3] = r_mat
trans_mat[:3, 3] = translation
trans_mat[3, 3] = 1
trans_mat = np.dot(cam_cali_mat, trans_mat)
trans_mat = inv(trans_mat)
return trans_mat
def trans_rot_to_hmat(trans, rot):
'''
Converts a rotation and translation to a homogeneous transform.
**Args:**
**trans (np.array):** Translation (x, y, z).
**rot (np.array):** Quaternion (x, y, z, w).
**Returns:**
H (np.array): 4x4 homogenous transform matrix.
'''
H = transformations.quaternion_matrix(rot)
H[0:3, 3] = trans
return H
def diff_vp(self,
verts,
cam=None,
angle=90,
axis=[1, 0, 0],
texture=None,
kp_verts=None,
new_ext=None,
extra_elev=False):
if cam is None:
cam = self.default_cam[0]
if new_ext is None:
new_ext = [0.6, 0, 0]
# Cam is 7D: [s, tx, ty, rot]
import cv2
cam = asVariable(cam)
quat = cam[-4:].view(1, 1, -1)
R = transformations.quaternion_matrix(
quat.squeeze().data.cpu().numpy())[:3, :3]
rad_angle = np.deg2rad(angle)
rotate_by = cv2.Rodrigues(rad_angle * np.array(axis))[0]
# new_R = R.dot(rotate_by)
new_R = rotate_by.dot(R)
if extra_elev:
# Left multiply the camera by 30deg on X.
R_elev = cv2.Rodrigues(np.array([np.pi / 9, 0, 0]))[0]
new_R = R_elev.dot(new_R)
# Make homogeneous
new_R = np.vstack(
[np.hstack((new_R, np.zeros((3, 1)))),
np.array([0, 0, 0, 1])])
new_quat = transformations.quaternion_from_matrix(new_R,
isprecise=True)
new_quat = Variable(torch.Tensor(new_quat).cuda(), requires_grad=False)
# new_cam = torch.cat([cam[:-4], new_quat], 0)
new_ext = Variable(torch.Tensor(new_ext).cuda(), requires_grad=False)
new_cam = torch.cat([new_ext, new_quat], 0)
rend_img = self.__call__(verts, cams=new_cam, texture=texture)
if kp_verts is None:
return rend_img
else:
kps = self.renderer.project_points(kp_verts.unsqueeze(0),
new_cam.unsqueeze(0))
kps = kps[0].data.cpu().numpy()
return kp2im(kps, rend_img, radius=1)
def mirror_image(self, img, mask, kp, sfm_pose):
kp_perm = self.kp_perm
if np.random.rand(1) > 0.5:
# Need copy bc torch collate doesnt like neg strides
img_flip = img[:, ::-1, :].copy()
mask_flip = mask[:, ::-1].copy()
# Flip kps.
new_x = img.shape[1] - kp[:, 0] - 1
kp_flip = np.hstack((new_x[:, None], kp[:, 1:]))
kp_flip = kp_flip[kp_perm, :]
# Flip sfm_pose Rot.
R = transformations.quaternion_matrix(sfm_pose[2])
flip_R = np.diag([-1, 1, 1, 1]).dot(R.dot(np.diag([-1, 1, 1, 1])))
sfm_pose[2] = transformations.quaternion_from_matrix(flip_R, isprecise=True)
# Flip tx
tx = img.shape[1] - sfm_pose[1][0] - 1
sfm_pose[1][0] = tx
return img_flip, mask_flip, kp_flip, sfm_pose
else:
return img, mask, kp, sfm_pose
def pub_ee_frame_velocity(self,
direction='z',
vel_scale=1.0,
duration_sec=0.1):
target_pos, target_quat = self.get_target_pose()
T = transformations.quaternion_matrix(target_quat)
T[:3, 3] = target_pos
dT = np.eye(4)
if direction == 'x':
dT[0, 3] = vel_scale * 0.005
if direction == 'y':
dT[1, 3] = vel_scale * 0.005
if direction == 'z':
dT[2, 3] = vel_scale * 0.005
T_post_vel = T.dot(dT)
new_target_pos = T_post_vel[:3, 3]
new_target_quat = transformations.quaternion_from_matrix(T_post_vel)
new_target_pose = np.concatenate([new_target_pos, new_target_quat])
self.set_target_pose(new_target_pose)
def quatswap(quat):
mat = transformations.quaternion_matrix(quat)
mat_new = np.c_[mat[:,2],mat[:,1],-mat[:,0],mat[:,3]]
return transformations.quaternion_from_matrix(mat_new)
# z: -0.363402061428
# w: -0.0133960769939
# create transformation matrix
# convert quaternions ([w,x,y,z])to transformation matrix
p2 = np.array([[-0.0641664267064, -0.0164370734761, 1.16176302976, 1]])
q2 = np.array(
[-0.211719271579, 0.0769653027773, 0.925899537619, -0.303251279382])
p1 = np.array([[0.00665347479368, -0.228122377393, 0.904231349513, 1]])
q1 = np.array(
[-0.0133960769939, 0.00252618701703, 0.931532664618, -0.363402061428])
Tw1 = tf.quaternion_matrix(q1)
Tw2 = tf.quaternion_matrix(q2)
# replace fourth column in T1 by the position
Tw1[:, 3] = p1
Tw2[:, 3] = p2
# compute relative transformation between Tw1 and Tw2
T12 = inv(Tw1) * Tw2
# get relative traslation
t12 = T12[:3, 3]
# get relative quaternion
R12 = T12[:3, :3]
q12 = tf.quaternion_from_matrix(T12)
def on_sensor_data(data):
#print('Sensor frame received')
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = np.asarray(rgb_image)
if rgb_image.shape != (256,256,3):
print('image shape not 256, 256, 3')
return None
# to save the received rgb images
# np.save(str(np.random.randint(9999)), rgb_image)
# only draw boxes every few seconds
if np.random.randint(45) > 41:
# cast a ray directly at the center of the image
# once this works we would need to vary this so that the centroid is not in the center of the image
# some errors will be suppressed by the symetry of this pixel choice, which is what I want at this moment.
ray = ray_casting.cast_ray(data, [128, 128])
# gimbal pose and quad pose: I am pretty sure they are in the world reference frame
# TODO remove this when sign flip on y is fixed in unity sim
gimbal_pose = tmpfix_position(list(map(float, data['gimbal_pose'].split(','))))
# TODO remove this when sign flip on y is fixed in unity sim
quad_pose = tmpfix_position(list(map(float, data['pose'].split(','))))
# the sim world has a static reference frame, the angle below is the angle between the world frame and the sensor frame
rot = transformations.euler_matrix(to_radians(gimbal_pose[3]),
to_radians(gimbal_pose[4]),
-to_radians(gimbal_pose[5]))[:3,:3]
# this is the identity rotation. The conventions of transformations, is w,x,y,z, and for unity x,y,z,w
gimbal_cam_rot = transformations.quaternion_matrix((1,0,0,0))[:3,:3]
# rotate the ray coordinates, so the ray is in the world frame. I think the order may matter here.
print('ray unrotated', ray)
ray = np.dot(gimbal_cam_rot, ray)
ray = np.dot(rot, np.array(ray))
print('ray rotated', ray)
# print some more debug data
euler = gimbal_pose[3:]
quaternion = transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
print('gimbal_rotation_quat', quaternion)
print('gimbal_pose_received', data['gimbal_pose'])
print('gimbal_pose_fixed', gimbal_pose)
euler = quad_pose[3:]
quaternion = transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
print('quad_rotation_quat', quaternion)
print('quad_position_received', data['pose'])
print('quad_pose_fixed', quad_pose)
# Flip the sign again so that that the pose received set by the sim is correct
# TODO remove the call to tmpfix when sign flip on y is fixed in unity sim
marker_pos = tmpfix_position((gimbal_pose[:3] + ray).tolist() + [0,0,0]).tolist()
marker_rot = [0,0,0]
quaternion = transformations.quaternion_from_euler(*marker_rot)
print('marker_rotation_quat', quaternion, '\n\n')
# prepare the marker message and send
marker_msg = sio_msgs.create_box_marker_msg(np.random.randint(99999), marker_pos)
socketIO.emit('create_box_marker', marker_msg)
def quat2mat(quat):
return transformations.quaternion_matrix(quat)[:3, :3]
linewidth=3.0))
handles.append(reach.ENV.drawlinestrip(points=states["xyz_r"],
linewidth=3.0))
handles.append(reach.ENV.drawlinestrip(points=states["xyz_l"],
linewidth=3.0,colors=(0,0,1)))
for i in xrange(len(base_xys)):
base_tf = np.eye(4)
base_tf[0:2,3] = base_xys[i]
xlarm,ylarm,zlarm = states["xyz_l"][i]
xrarm,yrarm,zrarm = states["xyz_r"][i]
larm_tf = tf.quaternion_matrix(states["quat_l"][i])
rarm_tf = tf.quaternion_matrix(states["quat_r"][i])
larm_tf[:3,3] += states["xyz_l"][i]
rarm_tf[:3,3] += states["xyz_r"][i]
with reach.ENV:
reach.PR2.SetTransform(base_tf)
dofvalues = reach.LEFT.FindIKSolution(larm_tf, 2+8)
if dofvalues is not None:
reach.PR2.SetDOFValues(dofvalues, reach.LEFT.GetArmIndices())
else:
print "left ik failed"
