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 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 transform_points(
xyz,
listener,
to_frame,
from_frame,
n_tries=10,
):
# listener.waitForTransform(to_frame, from_frame, rospy.Time.now(), rospy.Duration(1))
for i in xrange(n_tries):
try:
(trans, rot) = listener.lookupTransform(to_frame,
from_frame, rospy.Time(0))
break
except Exception:
print 'tf exception:'
print colorize(traceback.format_exc(), 'yellow')
rospy.sleep(.1)
time.sleep(.05)
if i == n_tries - 1:
raise Exception('f**k tf')
hmat = conversions.trans_rot_to_hmat(trans, rot)
xyz1 = np.c_[xyz.reshape(-1, 3), np.ones((xyz.size / 3, 1))]
xyz1_tf = np.dot(xyz1, hmat.T)
xyz_tf = xyz1_tf[:, :3].reshape(xyz.shape)
return xyz_tf
def transform_points(xyz, to_frame, from_frame):
xyz = xyz.reshape(-1,3)
xyz1 = np.c_[xyz, np.ones((xyz.shape[0],1))]
trans, rot = brett.tf_listener.lookupTransform(to_frame, from_frame,rospy.Time(0))
mat = conv.trans_rot_to_hmat(trans,rot)
xyz1_transformed = np.dot(xyz1, mat.T)
return xyz1_transformed[:,:3]
def update_rave(self, use_map=False):
ros_values = self.get_last_joint_message().position
rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]
self.robot.SetJointValues(rave_values[:20], self.rave_inds[:20])
self.robot.SetJointValues(rave_values[20:], self.rave_inds[20:])
if use_map:
(trans, rot) = self.tf_listener.lookupTransform("/map", "/base_link", rospy.Time(0))
self.robot.SetTransform(conv.trans_rot_to_hmat(trans, rot))
def transform_points(xyz, listener, to_frame, from_frame, n_tries=10):
#listener.waitForTransform(to_frame, from_frame, rospy.Time.now(), rospy.Duration(1))
for i in xrange(n_tries):
try:
trans, rot = listener.lookupTransform(to_frame, from_frame,
rospy.Time(0))
break
except Exception:
print "tf exception:"
print colorize.colorize(traceback.format_exc(), "yellow")
rospy.sleep(.1)
time.sleep(.05)
if i == n_tries - 1: raise Exception("f**k tf")
hmat = conversions.trans_rot_to_hmat(trans, rot)
xyz1 = np.c_[xyz.reshape(-1, 3), np.ones((xyz.size / 3, 1))]
xyz1_tf = np.dot(xyz1, hmat.T)
xyz_tf = xyz1_tf[:, :3].reshape(xyz.shape)
return xyz_tf
def make_joint_traj(xyzs, quats, arm, filter_options = 0):
"do ik and then fill in the points where ik failed"
n = len(xyzs)
assert len(quats) == n
joints = []
inds = []
for i in xrange(n):
joint = arm.manip.FindIKSolution(conv.trans_rot_to_hmat(xyzs[i], quats[i]), filter_options)
if joint is not None:
joints.append(joint)
inds.append(ind)
print "found ik soln for %i of %i points"%(len(inds), n)
joints2 = math_utils.interp2d(np.arange(n), inds, joints)
return joints2
def avg_transform (tfms):
"""
Averages transforms by converting to translations + quaternions.
Average out translations as normal (sum of vectors / # of vectors).
Average out quaternions as in avg_quaternions.
"""
if len(tfms) == 0:
#redprint("List empty for averaging transforms!")
return None
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in tfms]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans,axis=0)/trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
return conversions.trans_rot_to_hmat(avg_trans, avg_rot)
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 grasp_plate_from_cylinder(cylinder, tf_listener=None , larm_mover=None, rarm_mover=None, lgripper=None, rgripper=None, lr_force =None):
if tf_listener == None:
tf_listener = tf.TransformListener()
rospy.loginfo('created tf lisener...waiting 1 second')
rospy.sleep(1)
rospy.loginfo('done waiting')
if larm_mover == None:
larm_mover = pr2_am.ArmMover('left_arm')
if rarm_mover == None:
rarm_mover = pr2_am.ArmMover('right_arm')
if lgripper == None:
lgripper = gripper.Gripper('left_arm')
if rgripper == None:
rgripper = gripper.Gripper('right_arm')
global brett
if brett == None:
brett = PR2()
bottom_center = cylinder[0]
x = cylinder[0].point.x
y = cylinder[0].point.y
z = cylinder[0].point.z
r = cylinder[1]
h = cylinder[2]
tll_point = tf_listener.transformPose('torso_lift_link', bottom_center)
if tll_point.point.y > 0:
side = 'left'
arm_mover = larm_mover
gripper = lgripper
angles = np.linspace(math.pi/2,math.pi,math.pi/20)
arm = brett.larm
gripper_tool_frame = "l_gripper_tool_frame"
else:
side = 'right'
arm_mover = rarm_mover
gripper = rgripper
angles = np.linspace(math.pi,3.*math.pi/2.,math.pi/20)
arm = brett.rarm
gripper_tool_frame = "r_gripper_tool_frame"
for angle in angles:
q1 = quaternion_about_axis(math.pi/2,(0,1,0))
q2 = quaternion_about_axis(math.pi/2+angle,(1,0,0))
q3 = quaternion_about_axis(-outward_angle,(0,0,1))
q12 = quaternion_multiply(q1,q2)
q = quaternion_multiply(q12,q3)
pointing_axis = quaternion_matrix(q)[:3,0]
target = geometry_msgs.msg.PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = cylinder.header.frame_id
target.pose.position.x = x + math.cos(angle) * (r + offset_final) # - pointing_axis[0]*.08
target.pose.position.y = y - math.sin(angle) * (r + offset_final) # - pointing_axis[1]*.08
target.pose.position.z = z + offset_vertical # - pointing_axis[2]*.08 + half_height + .01
target.pose.orientation = Quaternion(q[0],q[1],q[2],q[3])
#TODO: check ik
import conversions
mat4 = conversions.trans_rot_to_hmat([target.pose.position.x, target.pose.position.y, target.pose.position.z],q)
joint_positions = arm.ik(mat4, target.header.frame_id, gripper_tool_frame)
ik_worked = (joint_positions is not None)
if not ik_worked:
continue
for offset in np.linspace(offset_init,offset_final,0.01):
target = geometry_msgs.msg.PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = cylinder.header.frame_id
target.pose.position.x = x + math.cos(angle) * (r + offset) # - pointing_axis[0]*.08
target.pose.position.y = y - math.sin(angle) * (r + offset) # - pointing_axis[1]*.08
target.pose.position.z = z + offset_vertical # - pointing_axis[2]*.08 + half_height + .01
target.pose.orientation = Quaternion(q[0],q[1],q[2],q[3])
try:
arm_mover.move_to_goal_directly(target,5.0,None,False,4,collision_aware=False)
except:
break
return grab_success(side[0])
return False
