def test_cup_pour(demo_name, exp_name): test_cup_pour_init() verb_data_accessor = multi_item_verbs.VerbDataAccessor(test=True) current_stage = 1 gripper_data_key = "r_gripper_tool_frame" # info and data for previous stage prev_demo_info = verb_data_accessor.get_stage_info(demo_name, current_stage-1) prev_demo_data = verb_data_accessor.get_demo_data(prev_demo_info.stage_name) prev_exp_info = verb_data_accessor.get_stage_info(exp_name, current_stage-1) prev_exp_data = verb_data_accessor.get_demo_data(prev_exp_info.stage_name) # info and data for current stage cur_demo_info = verb_data_accessor.get_stage_info(demo_name, current_stage) cur_demo_data = verb_data_accessor.get_demo_data(cur_demo_info.stage_name) cur_exp_info = verb_data_accessor.get_stage_info(exp_name, current_stage) cur_exp_data = verb_data_accessor.get_demo_data(cur_exp_info.stage_name) # point clouds of tool for demo and experiment prev_exp_pc = prev_exp_data["object_clouds"][prev_exp_info.item]["xyz"] cur_exp_pc = cur_exp_data["object_clouds"][cur_exp_info.item]["xyz"] # calculate the transformation from the world frame to the gripper frame prev_exp_gripper_pos = prev_exp_data[gripper_data_key]["position"][-1] prev_exp_gripper_orien = prev_exp_data[gripper_data_key]["orientation"][-1] prev_world_to_gripper_trans = np.linalg.inv(juc.trans_rot_to_hmat(prev_exp_gripper_pos, prev_exp_gripper_orien)) gripper_frame_trans = make_verb_traj.make_to_gripper_frame_hmat(prev_world_to_gripper_trans) warped_traj_resp = make_verb_traj.make_traj_multi_stage_do_work(cur_demo_info, [cur_exp_pc], None, current_stage, prev_demo_info, [prev_exp_pc], verb_data_accessor, gripper_frame_trans) # get the actual transformation between the old and new target objects (just a translation for this test) params = get_test_params() translation = params['translation'] actual_target_translation_matrix = jut.translation_matrix(translation) # get the demo special point trajectory cur_demo_gripper_traj_xyzs = cur_demo_data[gripper_data_key]["position"] cur_demo_gripper_traj_oriens = cur_demo_data[gripper_data_key]["orientation"] cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)] prev_demo_spec_pt_translation = jut.translation_matrix(np.array(prev_demo_info.special_point)) cur_demo_spec_pt_traj_as_mats = [np.dot(traj_mat, prev_demo_spec_pt_translation) for traj_mat in cur_demo_gripper_traj_mats] # get the expected experiment special point trajectory expected_spec_pt_traj = [np.dot(actual_target_translation_matrix, traj_mat) for traj_mat in cur_demo_spec_pt_traj_as_mats] # get the expected experiment gripper trajectory prev_exp_spec_pt_translation = jut.translation_matrix(np.array(prev_exp_info.special_point)) inv_cur_exp_spec_pt_translation = np.linalg.inv(prev_exp_spec_pt_translation) expected_gripper_traj = [np.dot(traj_mat, inv_cur_exp_spec_pt_translation) for traj_mat in expected_spec_pt_traj] # compare the expected new special point trajectory to the result of make_traj_multi_stage result_traj = warped_traj_resp.traj cur_exp_traj_as_mats = [juc.pose_to_hmat(pose) for pose in result_traj.r_gripper_poses.poses] report(similar_trajectories(expected_gripper_traj, cur_exp_traj_as_mats))
def get_demo_spec_pt_traj_mats(demo_tool_info, demo_target_data, gripper_data_key): demo_grip_traj_xyzs = demo_target_data[gripper_data_key]["position"] demo_grip_traj_oriens = demo_target_data[gripper_data_key]["orientation"] demo_grip_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_grip_traj_xyzs, demo_grip_traj_oriens)] demo_tool_spec_pt_translation = jut.translation_matrix(np.array(demo_tool_info.special_point)) demo_spec_pt_traj_mats = [np.dot(traj_mat, demo_tool_spec_pt_translation) for traj_mat in demo_grip_traj_mats] return demo_spec_pt_traj_mats
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.GetArmIndices() 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 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 __init__(self): PlannerPR2.__init__(self) rospy.wait_for_service('getCutLine') self.cutService = rospy.ServiceProxy('getCutLine',PointDir) rospy.wait_for_service('getHoleNormal') self.holeService = rospy.ServiceProxy('getHoleNormal', PointDir) self.camera_frame = '/camera_rgb_optical_frame' self.tf_listener.waitForTransform("/base_footprint", self.camera_frame, rospy.Time(), rospy.Duration(10.0)) (trans, rot) = self.tf_listener.lookupTransform('/base_footprint',self.camera_frame,rospy.Time(0)) # basefootprintFromCamera, assuming transform doesn't change. If it does, need to do this often. self.camera_transform = conv.trans_rot_to_hmat(trans, rot); # Initial index of cut picked up. Rest of procedure would be done taking this into account. self.init_index = 0 # Variables for flap pick-up # How far above the cut to start self.cut_upStartPos = 0.05 # How far to the side to start (+ve direction is opposite to the side of the cut) # i.e, for left cut, +ve is right and vice versa self.cut_sideStartPos = -0.00 # Rotation of gripper downward self.cut_rotStartPos = np.pi/4 # Initial gripper angle to hold cut self.cut_gripperStartAngle = 0.04 # How much to move down to pick up flap self.cut_moveDown = 0.1 # How much to move in to pick up flap self.cut_moveIn = 0.05 # How much to move up after picking up the flap self.cut_moveUpEnd = 0.045 # How much to move in after picking up flap self.cut_moveInEnd = 0.015 # Distance to move towards hole from start position to pierce self.hole_moveToward = 0.025 # Angle to move in circle to pierce cut self.hole_finAng = np.pi/3 # Angle to reverse in circle after piercing cut self.hole_returnAng = -np.pi/7 # The initial hole pierce point. This is assumed to be a point on the needle during the pierce. self.init_holePt = np.array([0,0,0]) # The final hole exit point. self.final_holePt = np.array([0,0,0]) # Angle to move in to pierce the second hole # self.secondPierce_angle = np.pi/1.9 # If needle is not moving back after first pierce self.secondPierce_angle = np.pi/2.5 # Distance behind the needle the regrasping starts. self.regrasp_initDist = 0.05 # Max effort for closing the gripper self.regrasp_closeMaxEffort = 80 # Angle to move through in order to pull out the needle self.regrasp_removalAng = np.pi/1.4
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 show_ik_solns(part_name, manip, i, warped_demo): frame = "%s_gripper_tool_frame"%part_name[0] hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i], warped_demo[frame]['orientation'][i]) solns = traj_ik_graph_search.ik_for_link(hmat, manip, frame, return_all_solns=True) # newrobots = [] print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i print solns
def lin_rigid_tps_transform(tps_transform, lin_point): lin_ag, trans_g, w_ng, x_na = tps_transform.lin_ag, tps_transform.trans_g, tps_transform.w_ng, tps_transform.x_na # orientation part is gradient(f) orien = tps.tps_grad(np.array([lin_point]), lin_ag, trans_g, w_ng, x_na)[0] # translation part is f(a) - gradient(f)*a trans = tps.tps_eval(np.array([lin_point]), lin_ag, trans_g, w_ng, x_na)[0] - np.dot(orien, lin_point) linearized_transform = juc.trans_rot_to_hmat(trans, juc.mat2quat(orthogonalize3_cross(np.array([orien])))) return linearized_transform
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, 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 get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform): demo_spec_pt_traj_xyzs, demo_spec_pt_traj_oriens = [], [] for demo_spec_pt_traj_mat in demo_spec_pt_traj_mats: demo_spec_pt_traj_xyz, demo_spec_pt_traj_orien = juc.hmat_to_trans_rot(demo_spec_pt_traj_mat) demo_spec_pt_traj_xyzs.append(demo_spec_pt_traj_xyz) demo_spec_pt_traj_oriens.append(juc.quat2mat(demo_spec_pt_traj_orien)) warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens = demo_to_exp_target_transform.transform_frames(np.array(demo_spec_pt_traj_xyzs), np.array(demo_spec_pt_traj_oriens)) warped_spec_pt_traj_mats = [juc.trans_rot_to_hmat(warped_spec_pt_traj_xyz, juc.mat2quat(warped_spec_pt_traj_orien)) for warped_spec_pt_traj_xyz, warped_spec_pt_traj_orien in zip(warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens)] return warped_spec_pt_traj_mats
def show_ik_solns(part_name, manip, i, warped_demo): frame = "%s_gripper_tool_frame" % part_name[0] hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i], warped_demo[frame]['orientation'][i]) solns = traj_ik_graph_search.ik_for_link(hmat, manip, frame, return_all_solns=True) # newrobots = [] print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i print solns
def compute_feas_inds(xyzs, quats, manip, targ_frame, check_collisions=False): assert(len(xyzs) == len(quats)) hmats = [conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)] link = manip.GetRobot().GetLink(targ_frame) Tcur_w_link = link.GetTransform() Tcur_w_ee = manip.GetEndEffectorTransform() Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee) def ikfunc(hmat): return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16 + (1 if check_collisions else 0)) return traj_ik_graph_search.compute_feas_inds(hmats, ikfunc)
def make_joint_traj_by_graph_search(xyzs, quats, manip, targ_frame, downsample=1, check_collisions=False): assert (len(xyzs) == len(quats)) hmats = [ conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats) ] ds_hmats = hmats[::downsample] orig_len, ds_len = len(hmats), len(ds_hmats) if downsample != 1: rospy.loginfo( 'Note: downsampled %s trajectory from %d points to %d points', manip.GetName(), orig_len, ds_len) link = manip.GetRobot().GetLink(targ_frame) Tcur_w_link = link.GetTransform() Tcur_w_ee = manip.GetEndEffectorTransform() Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee) def ikfunc(hmat): return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2 + 16 + (1 if check_collisions else 0)) def nodecost(joints): robot = manip.GetRobot() cost = 0 with robot: robot.SetDOFValues(joints, manip.GetArmIndices()) cost = 100 * robot.GetEnv().CheckCollision(robot) return cost start_joints = manip.GetRobot().GetDOFValues(manip.GetArmIndices()) paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint( ds_hmats, ikfunc, start_joints=start_joints, nodecost=nodecost if check_collisions else None) rospy.loginfo("%s: %i of %i points feasible", manip.GetName(), len(timesteps), ds_len) i_best = np.argmin(costs) rospy.loginfo("lowest cost of initial trajs: %f", costs[i_best]) path_init = unwrap_arm_traj(paths[i_best]) path_init_us = mu.interp2d(range(orig_len), range(orig_len)[::downsample], path_init) # un-downsample assert len(path_init_us) == orig_len return path_init_us, timesteps
def get_demo_spec_pt_traj_mats(demo_target_data, gripper_data_key): demo_grip_traj_xyzs = demo_target_data[gripper_data_key]["position"] demo_grip_traj_oriens = demo_target_data[gripper_data_key]["orientation"] demo_grip_traj_mats = [ juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_grip_traj_xyzs, demo_grip_traj_oriens) ] demo_tool_spec_pt_translation = jut.translation_matrix( np.array(demo_tool_info.special_point)) demo_spec_pt_traj_mats = [ np.dot(traj_mat, demo_tool_spec_pt_translation) for traj_mat in demo_grip_traj_mats ] return demo_spec_pt_traj_mats
def get_warped_grip_traj_mats(warped_spec_pt_traj_mats, tool_stage_data, demo_to_exp_tool_transform, spec_pt_in_grip, world_to_grip_transform_func, arm): if demo_to_exp_tool_transform is None: grip_to_spec_pt_transform = np.linalg.inv(jut.translation_matrix(spec_pt_in_grip)) else: demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm) demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip) demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world) # find the transformation from the new special point to the gripper frame warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame) warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame) warped_spec_pt_in_grip_trans = world_to_grip_transform_func(warped_spec_pt_in_world_trans) warped_spec_pt_in_grip_rot = warped_spec_pt_in_world_rot warped_spec_pt_in_grip_frame = juc.trans_rot_to_hmat(warped_spec_pt_in_grip_trans, warped_spec_pt_in_grip_rot) print_hmat_info(warped_spec_pt_in_grip_frame, "warped_spec_pt_in_grip_frame") # transform the warped special point trajectory back to a gripper trajectory in the experiment grip_to_spec_pt_transform = np.linalg.inv(warped_spec_pt_in_grip_frame) warped_grip_traj_mats = [np.dot(spec_pt_mat, grip_to_spec_pt_transform) for spec_pt_mat in warped_spec_pt_traj_mats] return warped_grip_traj_mats
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 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_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_by_graph_search(xyzs, quats, manip, targ_frame, downsample=1, check_collisions=False): assert(len(xyzs) == len(quats)) hmats = [conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)] ds_hmats = hmats[::downsample] orig_len, ds_len = len(hmats), len(ds_hmats) if downsample != 1: rospy.loginfo('Note: downsampled %s trajectory from %d points to %d points', manip.GetName(), orig_len, ds_len) link = manip.GetRobot().GetLink(targ_frame) Tcur_w_link = link.GetTransform() Tcur_w_ee = manip.GetEndEffectorTransform() Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee) def ikfunc(hmat): return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16 + (1 if check_collisions else 0)) #return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16) def nodecost(joints): robot = manip.GetRobot() cost = 0 with robot: robot.SetDOFValues(joints, manip.GetArmIndices()) cost = 100*robot.GetEnv().CheckCollision(robot) return cost start_joints = manip.GetRobot().GetDOFValues(manip.GetArmIndices()) paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint( ds_hmats, ikfunc, start_joints=start_joints #nodecost=nodecost if check_collisions else None ) rospy.loginfo("%s: %i of %i points feasible", manip.GetName(), len(timesteps), ds_len) i_best = np.argmin(costs) rospy.loginfo("lowest cost of initial trajs: %f", costs[i_best]) path_init = unwrap_arm_traj(paths[i_best]) path_init_us = mu.interp2d(range(orig_len), range(orig_len)[::downsample], path_init) # un-downsample assert len(path_init_us) == orig_len return path_init_us, timesteps
def make_joint_traj(xyzs, quats, joint_seeds, manip, ref_frame, targ_frame, filter_options): """ do ik to make a joint trajectory joint_seeds are the joint angles that this function will try to be close to I put that in so you could try to stay near the demonstration joint angles in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations (TODO) """ n = len(xyzs) assert len(quats) == n robot = manip.GetRobot() robot.SetActiveManipulator(manip.GetName()) joint_inds = manip.GetArmIndices() orig_joint = robot.GetDOFValues(joint_inds) joints = [] inds = [] for i in xrange(0, n): mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i]) robot.SetDOFValues(joint_seeds[i], joint_inds) 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.SetDOFValues(joint, joint_inds) robot.SetDOFValues(orig_joint, joint_inds) 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 make_joint_traj(xyzs, quats, joint_seeds,manip, ref_frame, targ_frame,filter_options): """ do ik to make a joint trajectory joint_seeds are the joint angles that this function will try to be close to I put that in so you could try to stay near the demonstration joint angles in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations (TODO) """ n = len(xyzs) assert len(quats) == n robot = manip.GetRobot() robot.SetActiveManipulator(manip.GetName()) joint_inds = manip.GetArmIndices() orig_joint = robot.GetDOFValues(joint_inds) joints = [] inds = [] for i in xrange(0,n): mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i]) robot.SetDOFValues(joint_seeds[i], joint_inds) 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.SetDOFValues(joint, joint_inds) robot.SetDOFValues(orig_joint, joint_inds) 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 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.GetArmIndices() 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 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])
robot = env.GetRobots()[0] manip = robot.SetActiveManipulator('leftarm') lower, upper = robot.GetDOFLimits( manip.GetArmIndices()) # get the limits of just the arm indices ikmodel = databases.inversekinematics.InverseKinematicsModel( robot, iktype=IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() n_iter = 1000 rand_joints = [] for i in range(n_iter): rand_joints.append(lower + numpy.random.rand(len(lower)) * (upper - lower)) rand_hmats = [ conv.trans_rot_to_hmat( *traj_opt.joints2xyzquat(manip, 'l_gripper_tool_frame', j)) for j in rand_joints ] start_time = time.time() solns = [] for hmat in Bar().iter(rand_hmats): s = numpy.array( traj_ik_graph_search.ik_for_link(hmat, manip, 'l_gripper_tool_frame', return_all_solns=True)) if s.size == 0: continue solns.append(s) end_time = time.time() print 'ik time: ', end_time - start_time, ' | ', (end_time -
def get_world_to_grip_exp_transform(exp_tool_data, gripper_data_key): exp_gripper_pos = exp_tool_data[gripper_data_key]["position"][-1] exp_gripper_orien = exp_tool_data[gripper_data_key]["orientation"][-1] world_to_grip_transform = np.linalg.inv(juc.trans_rot_to_hmat(exp_gripper_pos, exp_gripper_orien)) return world_to_grip_transform
def apply_tps_transform_to_hmat(tps_transform, hmat): xyz, quat = juc.hmat_to_trans_rot(hmat) warped_xyz, warped_mat = tps_transform.transform_frames(np.array([xyz]), np.array([juc.quat2mat(quat)])) return juc.trans_rot_to_hmat(warped_xyz[0], juc.mat2quat(warped_mat[0]))
import roslib roslib.load_manifest('tf') import tf import rospy import sensor_msgs.msg as sm import numpy as np from jds_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))
def get_world_to_grip_exp_transform(exp_tool_data, gripper_data_key): exp_gripper_pos = exp_tool_data[gripper_data_key]["position"][-1] exp_gripper_orien = exp_tool_data[gripper_data_key]["orientation"][-1] world_to_grip_transform = np.linalg.inv( juc.trans_rot_to_hmat(exp_gripper_pos, exp_gripper_orien)) return world_to_grip_transform
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], )
env=Environment() env.Load('robots/pr2-beta-static.zae') robot=env.GetRobots()[0] manip = robot.SetActiveManipulator('leftarm') lower,upper = robot.GetDOFLimits(manip.GetArmIndices()) # get the limits of just the arm indices ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() n_iter = 1000 rand_joints = [] for i in range(n_iter): rand_joints.append(lower+numpy.random.rand(len(lower))*(upper-lower)) rand_hmats = [conv.trans_rot_to_hmat(*traj_opt.joints2xyzquat(manip, 'l_gripper_tool_frame', j)) for j in rand_joints] start_time = time.time() solns = [] for hmat in Bar().iter(rand_hmats): s = numpy.array(traj_ik_graph_search.ik_for_link(hmat, manip, 'l_gripper_tool_frame', return_all_solns=True)) if s.size == 0: continue solns.append(s) end_time = time.time() print 'ik time: ', end_time-start_time, ' | ', (end_time-start_time)/n_iter, 'each' start_time = time.time() for i in Bar().iter(range(len(solns)-1)): a, b, c = traj_ik_graph_search.build_graph_part(None, solns[i], solns[i+1]) end_time = time.time()
import kinematics.reachability as kr import openravepy from jds_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 get_demo_grip_traj_mats(target_stage_data, arm): gripper_data_key = "%s_gripper_tool_frame" % (arm) demo_target_grip_traj_xyzs = target_stage_data[gripper_data_key]["position"] demo_target_grip_traj_oriens = target_stage_data[gripper_data_key]["orientation"] demo_target_grip_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_target_grip_traj_xyzs, demo_target_grip_traj_oriens)] return demo_target_grip_traj_mats
def get_demo_tool_grip_to_world_transform(tool_stage_data, arm): gripper_data_key = "%s_gripper_tool_frame" % (arm) demo_tool_grip_pos = tool_stage_data[gripper_data_key]["position"][-1] demo_tool_grip_orien = tool_stage_data[gripper_data_key]["orientation"][-1] demo_tool_grip_to_world_transform = juc.trans_rot_to_hmat(demo_tool_grip_pos, demo_tool_grip_orien) return demo_tool_grip_to_world_transform
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 jds_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))
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None): arms_used = current_stage_info.arms_used verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name) if stage_num == 0: # don't do any extra transformation for the first stage prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4) # no special point translation for first stage since no tool yet special_point_translation = np.identity(4) elif stage_num > 0: # make sure that the tool stage only uses one arm (the one with the tool) assert arms_used in ['r', 'l'] prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name) prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"] prev_exp_pc = prev_exp_clouds[0] prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02) prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02) gripper_data_key = "%s_gripper_tool_frame" % (arms_used) # transform point cloud in base frame to gripper frame # assume right hand has the tool for now # use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1] prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1] prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien) prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform) prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down]) # get the new point cloud in the new gripper frame # prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down] if to_gripper_frame_func is None: prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key) else: prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key) # get the transformation from the new point cloud to the old point cloud for the previous stage prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame) # transforms gripper trajectory point into special point trajectory point if prev_stage_info.special_point is None: # if there is no special point, linearize at origin prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3)) # don't do a special point translation if there is no specified special point special_point_translation = np.identity(4) else: prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point)) # translation from gripper pose in world frame to special point pose in world frame special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point)) if arms_used != 'b': arms_used_list = [arms_used] else: arms_used_list = ['r', 'l'] warped_stage_data = group_to_dict(verb_stage_data) # deep copy it resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used for arm in arms_used_list: gripper_data_key = "%s_gripper_tool_frame" % (arm) # find the special point trajectory before the target transformation cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"] cur_demo_gripper_traj_oriens = verb_stage_data[gripper_data_key]["orientation"] cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)] cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats] # find the transformation from the new special point to the gripper frame cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation)) # save the demo special point traj for plotting plot_spec_pt_traj = [] for gripper_mat in cur_demo_gripper_traj_mats: spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation)) plot_spec_pt_traj.append(spec_pt_xyz) print 'grip transform:' print prev_demo_to_exp_grip_transform_lin_rigid print 'special point translation:' print special_point_translation print 'inverse special point translation:' print cur_exp_inv_special_point_transformation # find the target transformation for the experiment scene demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()] if len(demo_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(demo_object_clouds[0], .02) y_md = voxel_downsample(cur_exp_clouds[0], .02) # transformation from old target object to new target object in world frame cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md) # apply the target warping transformation to the special point trajectory cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], [] for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats: cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat) cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz) cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien)) cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens)) plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)] # transform the warped special point trajectory back to a gripper trajectory in the experiment cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats] warped_stage_data[gripper_data_key]["position"] = [] warped_stage_data[gripper_data_key]["orientation"] = [] for exp_traj_mat in cur_exp_gripper_traj_mats: warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat) warped_stage_data[gripper_data_key]["position"].append(warped_pos) warped_stage_data[gripper_data_key]["orientation"].append(warped_orien) if arm == 'r': traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.r_gripper_poses.poses) traj.r_gripper_angles = warped_stage_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = clouds_frame_id elif arm == 'l': traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.l_gripper_poses.poses) traj.l_gripper_angles = warped_stage_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = clouds_frame_id Globals.handles = [] plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST)) return resp
from jds_utils.conversions import trans_rot_to_hmat,hmat_to_trans_rot from jds_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))