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 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 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 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 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)
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)
def rot_distance(quat1, quat2):
quat1_mat = jut.quaternion_matrix(quat1)
quat2_mat = jut.quaternion_matrix(quat2)
diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
return euclidean_dist(diff_quat, [0, 0, 0, 1])
def rot_distance(quat1, quat2):
quat1_mat = jut.quaternion_matrix(quat1)
quat2_mat = jut.quaternion_matrix(quat2)
diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
return euclidean_dist(diff_quat, [0, 0, 0, 1])
handles = []
handles.append(reach.ENV.drawlinestrip(points=base_xyzs, 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"
dofvalues = reach.RIGHT.FindIKSolution(rarm_tf, 2 + 8)
if dofvalues is not None:
def quat2mat(quat):
return transformations.quaternion_matrix(quat)[:3, :3]
def rot_distance(quat1, quat2):
quat1_mat = jut.quaternion_matrix(quat1)
quat2_mat = jut.quaternion_matrix(quat2)
diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
return (sum([x**2 for x in jut.euler_from_quaternion(diff_quat)]))**0.5
def quat2mat(quat):
return transformations.quaternion_matrix(quat)[:3, :3]
handles.append(reach.ENV.drawlinestrip(points=base_xyzs, 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"
dofvalues = reach.RIGHT.FindIKSolution(rarm_tf, 2 + 8)
if dofvalues is not None: