def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1,1,1)
marker.color = stdm.ColorRGBA(1,1,0,.5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint,clink) in U.child_map[linkname]:
markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
return markers
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
rospy.logdebug("joint: %s"%jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0,0,0]
if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
rospy.logdebug("joint: %s" % jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0, 0, 0]
if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]
parentFromChild = conversions.trans_rot_to_hmat(
joint.origin.position,
transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(
valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(
np.array(joint.axis) * valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,
joint.child,
ns=ns,
valuedict=valuedict)
def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
hmats = [rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats)]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [
rave.quatSlerp(quat_start, quat_end, t)
for t in np.linspace(0, 1, n_steps)
]
hmats = [
rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz])
for (xyz, quat) in zip(xyzs, quats)
]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def update_pose(self):
mat = trans.euler_matrix(*self.R)
mat[:3,3] = self.T
print self.T, self.R
ps = gm.PoseStamped()
ps.pose = hmat_to_pose(mat)
ps.header.frame_id = "/wide_stereo_gazebo_l_stereo_camera_optical_frame"
self.pose_pub.publish(ps)
if self.msg == None: return
xyz, rgb = pc2xyzrgb(self.msg)
xyz = xyz.reshape(-1,3)
xyz1 = np.c_[xyz, np.ones((xyz.shape[0],1))]
xyz1_transformed = np.dot(xyz1, mat.T)
pc = xyzrgb2pc(xyz1_transformed[:,:3].reshape(480,640,3), rgb, "/wide_stereo_gazebo_l_stereo_camera_optical_frame")
self.pub.publish(pc)
def make_kin_tree_from_link(ps, linkname, ns='default_ns', valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(
link.visual.geometry, urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type=Marker.MESH_RESOURCE, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(
link.visual.origin.position,
transformations.quaternion_from_euler(
*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1, 1, 1)
marker.color = stdm.ColorRGBA(1, 1, 0, .5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint, clink) in U.child_map[linkname]:
markers.extend(
make_kin_tree_from_joint(ps,
cjoint,
ns=ns,
valuedict=valuedict))
return markers
