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])