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 make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0):
"do ik and then fill in the points where ik failed"
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
joint_inds = manip.GetArmJoints()
robot.SetActiveDOFs(joint_inds)
orig_joint = robot.GetActiveDOFValues()
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetActiveDOFValues(joint)
robot.SetActiveDOFValues(orig_joint)
rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
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:"
traceback.print_exc()
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
from brett2 import ros_utils
import roslib;
roslib.load_manifest('tf')
import tf
import rospy
import sensor_msgs.msg as sm
import numpy as np
from utils import conversions
rospy.init_node("save_point_cloud_data")
listener = tf.TransformListener()
rospy.sleep(.5)
xyzs = []
while True:
response = raw_input("press enter when you're ready. type 'quit' when done")
if 'quit' in response: break
pc = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2)
trans,rot=listener.lookupTransform("base_footprint",pc.header.frame_id, rospy.Time(0))
hmat = conversions.trans_rot_to_hmat(trans,rot)
xyz, rgb = ros_utils.pc2xyzrgb(pc)
xyz = np.squeeze(xyz)
xyz1 = np.c_[xyz, np.ones((len(xyz),1))]
xyz1_base = np.dot(xyz1, hmat.T)
xyzs.append(xyz1_base[:,:3])
print "saving"
np.save("/home/joschu/Data/rope/ropes.npy",np.array(xyzs,dtype=object))
import kinematics.reachability as kr
import openravepy
from utils import conversions
import numpy as np
poses = []
poses.append(((0,.6,1), (0,0,0,1)))
poses.append(((0,.7,1), (0,0,0,1)))
poses.append(((0,.8,1), (0,0,0,1)))
env = openravepy.Environment()
env.Load("./tablescene.xml")
assert env is not None
robot = env.GetRobots()[0]
leftarm = robot.GetManipulator("leftarm")
db = kr.ReachabilityDatabase("read")
(x, y), cost = kr.find_min_cost_base(db, robot, "leftarm", poses, plotting=True)
print cost
base_pose = np.eye(4)
base_pose[0,3] = x
base_pose[1,3] = y
robot.SetTransform(base_pose)
mats = [conversions.trans_rot_to_hmat(trans,rot) for (trans,rot) in poses]
for mat in mats:
solns = leftarm.FindIKSolutions(mat,2+16)
print "num solutions",len(solns)
def callback(self,msg):
self.counter+= 1
if self.counter % 20 != 0:
return
if self.last_msg is None:
self.last_msg = msg
return
for i in xrange(2):
paddle = msg.paddles[i]
if (paddle.trigger>.5) and not (self.last_msg.paddles[i].trigger>.5):
xcur, ycur, zcur = self.arms[i].get_pose_matrix("torso_lift_link", "%s_gripper_tool_frame"%self.arms[i].lr)[:3,3]
xcmd, ycmd, zcmd = (paddle.transform.translation.x * self.scale,
paddle.transform.translation.y * self.scale,
paddle.transform.translation.z * self.scale)
self.xoffset, self.yoffset, self.zoffset = xcur-xcmd, ycur-ycmd, zcur-zcmd
print "engaging %s arm"%self.arms[i].lr
elif (paddle.trigger>.5) and (self.last_msg.paddles[i].trigger > .5):
x = paddle.transform.translation.x * self.scale + self.xoffset
y = paddle.transform.translation.y * self.scale + self.yoffset
z = paddle.transform.translation.z * self.scale + self.zoffset
xx = paddle.transform.rotation.x
yy = paddle.transform.rotation.y
zz = paddle.transform.rotation.z
ww = paddle.transform.rotation.w
joint_msg = JointCommand()
mat4 = conv.trans_rot_to_hmat([x,y,z], [xx,yy,zz,ww])
#print mat4
cur_joints = self.arms[i].get_joint_positions()
ik_joints = self.arms[i].cart_to_joint(mat4, "torso_lift_link", "r_gripper_tool_frame",filter_options=18)
self.arms[i].set_cart_target([xx,yy,zz,ww], [x,y,z], "torso_lift_link")
if ik_joints is not None:
ik_joints_unrolled = kinematics_utils.closer_joint_angles(ik_joints, cur_joints)
joint_msg.joints = ik_joints_unrolled
self.joint_pub.publish(joint_msg)
print "ik succeeded"
else:
print "ik failed"
#mat = transformations.quaternion_matrix([xx,yy,zz,ww])
#mat[:3,3] = [x,y,z]
#try: self.arms[i].goto_pose_matrix(mat, "torso_lift_link", self.arms[i].tool_frame);
#except IKFail: print "%s ik failed"%self.arms[i].lrlong
#self.grippers[i].set_angle_goal(.08*(1-paddle.trigger))
if paddle.buttons[1]: self.grippers[i].set_angle_goal(0)
if paddle.buttons[2]: self.grippers[i].set_angle_goal(.08)
if (msg.paddles[1].joy[0] > 0) or (msg.paddles[1].joy[1] > 0):
if msg.paddles[0].buttons[START]:
pan,tilt = self.pr2.head.get_joint_positions()
pan += .1*msg.paddles[1].joy[0]
tilt += .15*msg.paddles[1].joy[1]
self.pr2.head.set_pan_tilt(pan,tilt)
else:
self.pr2.base.set_twist((.1*msg.paddles[1].joy[1], -.1*msg.paddles[1].joy[0], msg.paddles[0].joy[0]))
self.last_msg = msg
from utils.conversions import trans_rot_to_hmat,hmat_to_trans_rot
from utils.transformations import euler_from_quaternion
import rospy
import roslib
roslib.load_manifest("tf")
import tf
import numpy as np
class Globals:
listener = None
if rospy.get_name() == "/unnamed":
rospy.init_node("transform_kinect_frames")
Globals.listener = tf.TransformListener()
rospy.sleep(1)
ONIfromCL = trans_rot_to_hmat(*Globals.listener.lookupTransform("/camera_rgb_optical_frame","camera_link", rospy.Time(0)))
GAZfromONI = trans_rot_to_hmat([0, -.15, -.24], [0, 0, 0, 1])
HEADfromGAZ = trans_rot_to_hmat(*Globals.listener.lookupTransform("/head_plate_frame", "/wide_stereo_gazebo_l_stereo_camera_optical_frame",rospy.Time(0)))
HEADfromCL = np.dot(np.dot(HEADfromGAZ, GAZfromONI), ONIfromCL)
trans_final, quat_final = hmat_to_trans_rot(HEADfromCL)
eul_final = euler_from_quaternion(quat_final)
print "translation", trans_final
print "rotation", eul_final
print "%.4f %.4f %.4f %.4f %.4f %.4f %.4f"%(tuple(trans_final) + tuple(quat_final))
def ros_transform_to_hmat(tros):
return conversions.trans_rot_to_hmat(
[tros.translation.x, tros.translation.y, tros.translation.z],
[tros.rotation.x, tros.rotation.y, tros.rotation.z, tros.rotation.w])
