def test_baxter_ik(self): env = ParamSetup.setup_env() baxter = ParamSetup.setup_baxter() can = ParamSetup.setup_green_can(geom=(0.02, 0.25)) baxter_body = OpenRAVEBody(env, baxter.name, baxter.geom) can_body = OpenRAVEBody(env, can.name, can.geom) baxter_body.set_transparency(0.5) can_body.set_transparency(0.5) manip = baxter_body.env_body.GetManipulator('right_arm') robot = baxter_body.env_body can = can_body.env_body dof = robot.GetActiveDOFValues() #Open the Gripper so there won't be collisions between gripper and can dof[9], dof[-1] = 0.02, 0.02 robot.SetActiveDOFValues(dof) iktype = IkParameterizationType.Transform6D thetas = np.linspace(0, np.pi * 2, 10) target_trans = OpenRAVEBody.transform_from_obj_pose([0.9, -0.23, 0.93], [0, 0, 0]) can_body.env_body.SetTransform(target_trans) target_trans[:3, :3] = target_trans[:3, :3].dot( matrixFromAxisAngle([0, np.pi / 2, 0])[:3, :3]) can_trans = target_trans body_trans = np.eye(4) """ To check whether baxter ik model works, uncomment the following """ # env.SetViewer('qtcoin') for theta in thetas: can_trans[:3, :3] = target_trans[:3, :3].dot( matrixFromAxisAngle([theta, 0, 0])[:3, :3]) solution = manip.FindIKSolutions( IkParameterization(can_trans, iktype), IkFilterOptions.CheckEnvCollisions) if len(solution) > 0: print "Solution found with pose and rotation:" print OpenRAVEBody.obj_pose_from_transform(can_trans) else: print "Solution not found with pose and rotation:" print OpenRAVEBody.obj_pose_from_transform(can_trans) for sols in solution: dof[10:17] = sols robot.SetActiveDOFValues(dof) time.sleep(.2) body_trans[:3, :3] = can_trans[:3, :3].dot( matrixFromAxisAngle([0, -np.pi / 2, 0])[:3, :3]) can_body.env_body.SetTransform(body_trans) self.assertTrue( np.allclose([0.9, -0.23, 0.93], manip.GetTransform()[:3, 3]))
def __init__(self): self.env = openravepy.Environment() self.env.StopSimulation() self.env.Load(osp.join(cad_files_dir, 'table_sim.xml')) self.env.Load("robots/pr2-beta-static.zae") self.env.SetViewer('qtcoin') print self.env.GetViewer() self.robot = self.env.GetRobots()[0] self.cam_tfm = openravepy.matrixFromAxisAngle(np.pi*np.array([-0.7,0.7,0])) self.cam_tfm = openravepy.matrixFromAxisAngle([0,0,np.pi]).dot(self.cam_tfm) self.cam_tfm[:3,3] = np.array([0.5,0.0, 2]) self.rope = None
def PlaceObjectRandomOrientation(self, objectName, scale=1): '''Places the specified object in the center with a (semi-)random orientation.''' with self.env: # load object into environment self.env.Load(objectName, {'scalegeometry': str(scale)}) idx1 = objectName.rfind("/") idx2 = objectName.rfind(".") shortObjectName = objectName[idx1 + 1:idx2] obj = self.env.GetKinBody(shortObjectName) # set physical properties l = obj.GetLinks()[0] l.SetMass(1) l.SetStatic(False) # choose a semi-random orientation orientOptions = [\ [(1,0,0), pi/2, (0, 1,0), 1],\ [(1,0,0), -pi/2, (0,-1,0), 1],\ [(0,1,0), pi/2, (-1,0,0), 0],\ [(0,1,0), -pi/2, ( 1,0,0), 0],\ [(0,0,1), 0, (0,0, 1), 2], [(1,0,0), pi, (0,0,-1), 2]] # [axis, angle, newAxis, newAxisIndex] optionIndex = randint(0, len(orientOptions)) orientOption = orientOptions[optionIndex] randomAngle = 2 * pi * rand() R1 = openravepy.matrixFromAxisAngle(orientOption[0], orientOption[1]) R2 = openravepy.matrixFromAxisAngle(orientOption[2], randomAngle) # adjust position to be above table boundingBox = obj.ComputeAABB().extents() T = eye(4) T[2, 3] = boundingBox[orientOption[3]] + self.tableExtents[2] # set object transform # print orientOption, randomAngle*(180/pi), boundingBox[orientOption[3]] objPose = dot(T, dot(R1, R2)) obj.SetTransform(objPose) return obj, objPose
def rotate_by(self, ang): #TODO Fill in, remove sleep #time.sleep(0) T = openravepy.matrixFromAxisAngle([0,0,ang]) # Get the transformation matrix with self.env: self.robot.SetTransform(np.dot(T,self.robot.GetTransform())) # Applying the new transformation
def get_ik_transform(pos, rot): trans = OpenRAVEBody.transform_from_obj_pose(pos, rot) # Openravepy flip the rotation axis by 90 degree, thus we need to change it back rot_mat = matrixFromAxisAngle([0, PI / 2, 0]) trans_mat = trans[:3, :3].dot(rot_mat[:3, :3]) trans[:3, :3] = trans_mat return trans
def generate_random_pos(robot, obj_to_grasp = None): """Generate a random position for the robot within the boundaries of the world. """ T = robot.GetTransform() if obj_to_grasp is None: max_x = 2.5 max_y = 2.5 max_th = np.pi else: obj_pos = obj_to_grasp.GetTransform()[:3,-1] max_x = obj_pos[0] - 1.0 max_y = obj_pos[1] - 1.0 x = np.random.uniform(-max_x, max_x) y = np.random.uniform(-max_y, max_y) z = T[2,3] if obj_to_grasp is None: th = np.random.uniform(-max_th, max_th) else: robot_pos = T[:3,-1] th = np.arctan2(obj_pos[1] -y , obj_pos[0] - x) #rotation T = openravepy.matrixFromAxisAngle([0,0,th]) #translation T[:, -1] = [x, y, z, 1] return T
def rotate_by(self, ang): # TODO Fill in, remove sleep # Tz = np.matrix('0 1 0;-1 0 0; 0 0 1') Tz = openravepy.matrixFromAxisAngle([0, 0, ang]) with self.env: self.robot.SetTransform(np.dot(self.robot.GetTransform(), Tz)) time.sleep(0)
def get_ee_transform_from_pose(pose, rotation): ee_trans = OpenRAVEBody.transform_from_obj_pose(pose, rotation) #the rotation is to transform the tool frame into the end effector transform rot_mat = matrixFromAxisAngle([0, PI / 2, 0]) ee_rot_mat = ee_trans[:3, :3].dot(rot_mat[:3, :3]) ee_trans[:3, :3] = ee_rot_mat return ee_trans
def SimulateObjectMovementOnClose(self, descriptor, obj): '''The object can move when the fingers close during a grasp. This sets the object to an approximation to the correct resultant pose. - Input descriptor: Grasp pose. Assumes this is a valid grasp. - Input obj: The object being grasped. - Returns None. ''' # get object pose in hand frame bTo = obj.GetTransform() hTb = inv(descriptor.T) hTo = dot(hTb, bTo) if self.IsBottleUpright(obj): # Top grasp. Simply set the y-position to 0. hTo[1, 3] = 0 else: # Side grasp. # Set y = 0 at the point of contact along the bottle axis. alpha = -hTo[0, 3] / hTo[0, 2] deltaY = hTo[1, 3] + alpha * hTo[1, 2] hTo[1, 3] -= deltaY # Set the orientation to be vertical in the hand. zAxis = hTo[0:2, 2] if hTo[0, 2] >= 0 else -hTo[0:2, 2] angle = arccos(dot(zAxis, array([1.0, 0.0])) / norm(zAxis)) angle = angle if zAxis[1] <= 0 else 2 * pi - angle handDepthAxis = array([0.0, 0.0, 1.0]) T = openravepy.matrixFromAxisAngle(handDepthAxis, angle) hTo = dot(T, hTo) # set the object's new pose in the base frame bToNew = dot(descriptor.T, hTo) obj.SetTransform(bToNew)
def main(): global m global e e = r.Environment() m = r.RaveCreateModule(e, 'orchomp') e.SetViewer('qtcoin') e.Load("intelkitchen_robotized_herb2_nosensors.env.xml") robot = e.GetRobot("Herb2") # set the manipulator to leftarm #ikmodel = databases.inversekinematics.InverseKinematicsModel( # robot,iktype=IkParameterization.Type.Transform6D) #if not ikmodel.load(): # ikmodel.autogenerate() Tz = r.matrixFromAxisAngle([0, 0, numpy.pi / 2]) Tz[0, 3] = 0.4 Tz[1, 3] = 1.6 Tz[2, 3] = -0.01 print Tz with e: for body in e.GetBodies(): body.SetTransform(numpy.dot(Tz, body.GetTransform())) readFileCommands()
def observe_cloud2(self, radius, axis_upsample=0, radius_sample=0, angle_sample=0): axis_pts = self.rope.GetControlPoints() indices = range(len(axis_pts)) if axis_upsample != 0: lengths = np.r_[0, self.rope.GetHalfHeights() * 2] summed_lengths = np.cumsum(lengths) assert len(lengths) == len(axis_pts) indices = np.interp(np.linspace(0, summed_lengths[-1], axis_upsample*len(lengths)), summed_lengths, indices) axis_pts = math_utils.interp2d(np.linspace(0, summed_lengths[-1], axis_upsample*len(lengths)), summed_lengths, axis_pts) half_heights = self.rope.GetHalfHeights() rotates = self.rope.GetRotations() translates = self.rope.GetTranslations() new_pts = [] for r in range(1, radius_sample+1): d = r * radius / float(radius_sample) for a in range(angle_sample): local_rotate = matrixFromAxisAngle([a/float(angle_sample)*2*np.pi,0,0])[:3,:3] for index in indices: node_id = np.min([np.floor(index), len(self.rope.GetNodes()) - 1]) h = half_heights[node_id] v = np.array([-h + 2*h*(index-node_id),d,0]) rotate = rotates[node_id] translate = translates[node_id] v = rotate.dot(local_rotate.dot(v)) + translate new_pts.append(v) for pt in axis_pts: new_pts.append(pt) return np.array(new_pts)
def test_grid_to_trajectories(self): points = array([2831, 2832, 2833, 2777, 2778, 2779, 2780]) borders = array([[-0.34395401, 0.78002898, -3.65800373], [0.04986939, -0.00596685, -3.75569145]]) point_vert = array([-0.94672199, 0.76057971, -1.52572671, -0.82705471, -0.51077813, -0.23470453]) T = matrixFromAxisAngle([pi / 4, 0, 0]) gtt_00 = self.DB_00.load_grid_to_trajectories() self.assertTrue(isinstance(gtt_00,dict), msg='grid_to_trajectories 00 should be a dictionary') self.assertTrue(len(gtt_00[0])==2, msg='grid_to_trajectories[0] should be a size 2 list (trajectories and borders)') self.assertTrue(len(gtt_00[0][0]) == 151, msg='grid_to_trajectories[0][0] should have 151 trajectories') assert_allclose(gtt_00[0][0][0], points) assert_allclose(gtt_00[0][1][0], borders) borders_45 = [dot(T[0:3,0:3],gtt_00[0][1][0][0]),dot(T[0:3,0:3],gtt_00[0][1][0][1])] gtt_45 = self.DB_45.load_grid_to_trajectories() self.assertTrue(isinstance(gtt_45, dict), msg='grid_to_trajectories 45 should be a dictionary') self.assertTrue(len(gtt_45[0]) == 2, msg='grid_to_trajectories[0] should be a size 2 list (trajectories and borders)') self.assertTrue(len(gtt_45[0][0]) == 151, msg='grid_to_trajectories[0][0] should have 151 trajectories') assert_allclose(gtt_45[0][0][0], points) assert_allclose(gtt_45[0][1][0], borders_45) gtt_vert = self.DB_VERT.load_grid_to_trajectories() self.assertTrue(isinstance(gtt_vert, dict), msg='grid_to_trajectories vert should be a dictionary') self.assertTrue(len(gtt_vert[64]) == 26, msg='grid_to_trajectories[0] should be a size 26 list (trajectories)') self.assertTrue(len(gtt_vert[64][0]) == 118, msg='grid_to_trajectories[0][0] should have 118 points') assert_allclose(gtt_vert[64][0][0], point_vert)
def observe_cloud(pts, radius, upsample=0, upsample_rad=1): """ If upsample > 0, the number of points along the rope's backbone is resampled to be upsample points If upsample_rad > 1, the number of points perpendicular to the backbone points is resampled to be upsample_rad points, around the rope's cross-section The total number of points is then: (upsample if upsample > 0 else len(self.rope.GetControlPoints())) * upsample_rad """ if upsample > 0: lengths = np.r_[0, np.apply_along_axis(np.linalg.norm, 1, np.diff(pts, axis=0))] summed_lengths = np.cumsum(lengths) assert len(lengths) == len(pts) pts = math_utils.interp2d(np.linspace(0, summed_lengths[-1], upsample), summed_lengths, pts) if upsample_rad > 1: # add points perpendicular to the points in pts around the rope's cross-section vs = np.diff(pts, axis=0) # vectors between the current and next points vs /= np.apply_along_axis(np.linalg.norm, 1, vs)[:,None] perp_vs = np.c_[-vs[:,1], vs[:,0], np.zeros(vs.shape[0])] # perpendicular vectors between the current and next points in the xy-plane perp_vs /= np.apply_along_axis(np.linalg.norm, 1, perp_vs)[:,None] vs = np.r_[vs, vs[-1,:][None,:]] # define the vector of the last point to be the same as the second to last one perp_vs = np.r_[perp_vs, perp_vs[-1,:][None,:]] # define the perpendicular vector of the last point to be the same as the second to last one perp_pts = [] from openravepy import matrixFromAxisAngle for theta in np.linspace(0, 2*np.pi, upsample_rad, endpoint=False): # uniformly around the cross-section circumference for (center, rot_axis, perp_v) in zip(pts, vs, perp_vs): rot = matrixFromAxisAngle(rot_axis, theta)[:3,:3] perp_pts.append(center + rot.T.dot(radius * perp_v)) pts = np.array(perp_pts) return pts
def generate_random_pos(robot, obj_to_grasp=None): """Generate a random position for the robot within the boundaries of the world. """ T = robot.GetTransform() if obj_to_grasp is None: max_x = 2.5 max_y = 2.5 max_th = np.pi else: obj_pos = obj_to_grasp.GetTransform()[:3, -1] max_x = obj_pos[0] - 1.0 max_y = obj_pos[1] - 1.0 x = np.random.uniform(-max_x, max_x) y = np.random.uniform(-max_y, max_y) z = T[2, 3] if obj_to_grasp is None: th = np.random.uniform(-max_th, max_th) else: robot_pos = T[:3, -1] th = np.arctan2(obj_pos[1] - y, obj_pos[0] - x) #rotation T = openravepy.matrixFromAxisAngle([0, 0, th]) #translation T[:, -1] = [x, y, z, 1] return T
def get_ik_transform(pos, rot): trans = OpenRAVEBody.transform_from_obj_pose(pos, rot) # Openravepy flip the rotation axis by 90 degree, thus we need to change it back rot_mat = matrixFromAxisAngle([0, np.pi/2, 0]) trans_mat = trans[:3, :3].dot(rot_mat[:3, :3]) trans[:3, :3] = trans_mat return trans
def set_camera(orviewer, orcamparams): root = xml.etree.ElementTree.fromstring('<root>{}</root>'.format(orcamparams)) ra = map(float,root.find('camrotationaxis').text.split()) axisangle = numpy.array(ra[0:3]) * ra[3] * numpy.pi / 180.0 H = openravepy.matrixFromAxisAngle(axisangle) H[0:3,3] = map(float,root.find('camtrans').text.split()) e.GetViewer().SetCamera(H, float(root.find('camfocal').text))
def SampleActions(self): '''TODO''' if self.nPlaceOrientations != 3: raise Exception( "Currently, exactly 3 orientations only supported for place.") y90 = openravepy.matrixFromAxisAngle([0.0, 1.0, 0.0], -pi / 2.0)[0:3, 0:3] x180 = openravepy.matrixFromAxisAngle([1.0, 0.0, 0.0], pi)[0:3, 0:3] R1 = y90 # approach aligned with world x R2 = eye(3) # approach aligned with world -z R3 = dot(x180, y90) # R1 flipped 180 about approach return [R1, R2, R3]
def ik_solution(self, manip, iktype, ee_pos, ee_rot): ee_trans = OpenRAVEBody.transform_from_obj_pose(ee_pos, ee_rot) # Openravepy flip the rotation axis by 90 degree, thus we need to change it back rot_mat = matrixFromAxisAngle([0, np.pi/2, 0]) ee_trans = ee_trans.dot(rot_mat) solutions = manip.FindIKSolutions(IkParameterization(ee_trans,iktype),IkFilterOptions.CheckEnvCollisions) return solutions
def ik_solution(self, manip, iktype, ee_pos, ee_rot): ee_trans = OpenRAVEBody.transform_from_obj_pose(ee_pos, ee_rot) # Openravepy flip the rotation axis by 90 degree, thus we need to change it back rot_mat = matrixFromAxisAngle([0, np.pi / 2, 0]) ee_trans = ee_trans.dot(rot_mat) solutions = manip.FindIKSolutions(IkParameterization(ee_trans, iktype), IkFilterOptions.CheckEnvCollisions) return solutions
def load_segment(demofile, segment, fake_data_transform=[0, 0, 0, 0, 0, 0]): fake_seg = demofile[segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(fake_data_transform[3:6]) # @UndefinedVariable hmat[:3, 3] = fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) return new_xyz, r2r
def load_fake_data_segment(demofile, fake_data_segment, fake_data_transform): fake_seg = demofile[fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"])[:, :3] hmat = openravepy.matrixFromAxisAngle(fake_data_transform[3:6]) hmat[:3, 3] = fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] joint_names = asarray(fake_seg["joint_states"]["name"]) joint_values = asarray(fake_seg["joint_states"]["position"][0]) return new_xyz, joint_names, joint_values
def sampling(self): """ The sampling method is an algorithm for uniformly sampling objects. It computes the bounding box of the object (object inscribed by cube), uniformly samples the box's faces, and checks rays collision from cube's samples to the object. Rays with collision and its normal vectors are stored as object's samples. Finally, a kdtree is computed from object's samples and neighbooring points are removed to generate an uniformly sampling on the object (filtering data by distance). This is a data-intensive computing and might freeze your computer. """ delta = self.samples_delta min_distance_between_points = self.min_distance_between_points cc = RaveCreateCollisionChecker(self.turbine.env, 'ode') if cc is not None: ccold = self.turbine.env.GetCollisionChecker() self.turbine.env.SetCollisionChecker(cc) cc = ccold self._blade.SetTransform(eye(4)) self._blade.SetTransform(dot(self._blade.GetTransform(), matrixFromAxisAngle([0, -self.turbine.config.environment.blade_angle, 0])) ) ab = self._blade.ComputeAABB() p = ab.pos() e = ab.extents() + 0.01 # increase since origin of ray should be outside of object sides = array(( (e[0], 0, 0, -1, 0, 0, 0, e[1], 0, 0, 0, e[2]), # x (-e[0], 0, 0, 1, 0, 0, 0, e[1], 0, 0, 0, e[2]), # -x (0, 0, -e[2], 0, 0, 1, e[0], 0, 0, 0, e[1], 0), # -z (0, 0, e[2], 0, 0, -1, e[0], 0, 0, 0, e[1], 0), # z (0, e[1], 0, 0, -1, 0, e[0], 0, 0, 0, 0, e[2]), # y (0, -e[1], 0, 0, 1, 0, e[0], 0, 0, 0, 0, e[2]) # -y )) maxlen = 2 * sqrt(sum(e ** 2)) + 0.03 self.points = zeros((0, 6)) for side in sides: ex = sqrt(sum(side[6:9] ** 2)) ey = sqrt(sum(side[9:12] ** 2)) XX, YY = meshgrid(r_[arange(-ex, -0.25 * delta, delta), 0, arange(delta, ex, delta)], r_[arange(-ey, -0.25 * delta, delta), 0, arange(delta, ey, delta)]) localpos = outer(XX.flatten(), side[6:9] / ex) + outer(YY.flatten(), side[9:12] / ey) N = localpos.shape[0] rays = c_[tile(p + side[0:3], (N, 1)) + localpos, maxlen * tile(side[3:6], (N, 1))] collision, info = self.turbine.env.CheckCollisionRays(rays, self._blade) # make sure all normals are the correct sign: pointing outward from the object) newinfo = info[collision, :] if len(newinfo) > 0: newinfo[sum(rays[collision, 3:6] * newinfo[:, 3:6], 1) > 0, 3:6] *= -1 self.points = r_[self.points, newinfo] self.points = mathtools.filter_by_distance(self.points, min_distance_between_points) self.samples_delta = delta self.min_distance_between_points = min_distance_between_points return
def test_get_sorted_points(self): point_0 = (-0.152195798, 0.538141088, -3.765726656) T = matrixFromAxisAngle([pi / 4, 0, 0]) ntp_00 = self.DB_00.get_sorted_points() assert_allclose(ntp_00[0], point_0) point_45 = dot(T[0:3,0:3], point_0) ntp_45 = self.DB_45.get_sorted_points() assert_allclose(ntp_45[0], point_45)
def load_fake_data_segment(demofile, fake_data_segment, fake_data_transform): fake_seg = demofile[fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"])[:,:3] hmat = openravepy.matrixFromAxisAngle(fake_data_transform[3:6]) hmat[:3,3] = fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] joint_names = asarray(fake_seg["joint_states"]["name"]) joint_values = asarray(fake_seg["joint_states"]["position"][0]) return new_xyz, joint_names, joint_values
def load_fake_data_segment(demofile, fake_data_segment, fake_data_transform, set_robot_state=True): fake_seg = demofile[fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(fake_data_transform[3:6]) hmat[:3,3] = fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) if set_robot_state: r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) return new_xyz, r2r
def getTransform(self, cfg=None): Placement = eye(4) Placement[0:3, 3] = self.getXYZ(cfg) if cfg is not None: Placement[0:3, 3] += [ 0, 0, 2.0 * cfg.environment.robot_level_difference ] R = matrixFromAxisAngle( [0, 0, self.alpha + (sign(self.p) + 1) * pi / 2.0]) return dot(Placement, R)
def get_ee_transform_from_pose(pose, rotation): """ This helper function that returns the correct end effector rotation axis (perpendicular to gripper side) """ ee_trans = OpenRAVEBody.transform_from_obj_pose(pose, rotation) #the rotation is to transform the tool frame into the end effector transform rot_mat = matrixFromAxisAngle([0, PI / 2, 0]) ee_rot_mat = ee_trans[:3, :3].dot(rot_mat[:3, :3]) ee_trans[:3, :3] = ee_rot_mat return ee_trans
def test_draw_plane(self): np.random.seed(123) axis = np.random.randn(3) axis /= np.linalg.norm(axis) angle = np.deg2rad(45) transform = orpy.matrixFromAxisAngle(angle * axis) transform[:3, 3] = np.random.randn(3) * 0.5 h = ru.visual.draw_plane(self.env, transform) if self.display_available: self.assertEqual(type(h), orpy.GraphHandle)
def test_compute_robot_joints(self): """ The test generates a trajectory and computes robot joints. """ turbconf = TurbineConfig.load(self.path, self.test_dir) turb = Turbine(turbconf) robot_pos = turb.robot.GetTransform()[0:3,3] theta = arange(-pi/10,pi/10,0.001) # Generating positions x = -2.7 + (2 - cos(3*theta))*cos(theta) y = 1 + (2 - cos(3*theta))*sin(theta) z = zeros(len(theta)) positions = [[x[i],y[i],z[i]] for i in range(0,len(x))] # Generating normal vectors ny = 2*cos(theta)+cos(2*theta)-2*cos(4*theta) nx = -2*sin(theta)+sin(2*theta)+2*sin(4*theta) nz = zeros(len(theta)) normal_vectors = array([[nx[i],ny[i],nz[i]] for i in range(0,len(nx))]) normal_vectors = normal_vectors*(1.0/sqrt(sum(normal_vectors*normal_vectors,1))).reshape(len(normal_vectors),1) for i in range(0,len(normal_vectors)): normal_vectors[i] = cross(normal_vectors[i],[0,0,1]) trajectory = [[positions[i][0], positions[i][1], positions[i][2], normal_vectors[i][0], normal_vectors[i][1], normal_vectors[i][2]] for i in range(0,len(positions))] X90 = matrixFromAxisAngle([pi/2, 0, 0]) Z180 = matrixFromAxisAngle([0, 0, pi]) trajectory = array(mathtools.rotate_trajectories(turb, [trajectory], dot(Z180,X90))[0]) trajectory[:,2] = trajectory[:,2] + robot_pos[2] ## from .. import visualizer ## vis = visualizer.Visualizer(turb.env) ## vis.plot(array(trajectory)[:,0:3]) ## vis.plot_normal(trajectory) ## x = raw_input('wait') joint_solutions = robot_utils.compute_robot_joints(turb, trajectory, 0) self.assertTrue(len(joint_solutions)==len(trajectory), msg = 'The trajectory is not feasible')
def SampleActions(self): '''TODO''' theta = linspace(-pi / 2.0, pi / 2.0, self.nGraspOrientations) axis = array([0.0, 0.0, 1.0]) rotations = [] for t in theta: R = openravepy.matrixFromAxisAngle(axis, t)[0:3, 0:3] rotations.append(R) return rotations
def main(): for i in range(1): """ Tz = r.matrixFromAxisAngle([0,0,numpy.pi/2]) Tz[0,3] = 0.4 Tz[1,3] = 1.6 Tz[2,3] = -0.01 """ Tz = r.matrixFromAxisAngle([0,0,0]) Tz[2,3] = 0.01 runtrial( Tz , False, True, False )
def find_ik_solutions(robot, target, iktype, collision_free=True, freeinc=0.1): """ Find all the possible IK solutions Parameters ---------- robot: orpy.Robot The OpenRAVE robot target: array_like or orpy.Ray The target in the task space (Cartesian). A target can be defined as a Ray (position and direction) which is 5D or as an homogeneous transformation which is 6D. iktype: orpy.IkParameterizationType Inverse kinematics type to use. Supported values: `orpy.IkParameterizationType.Transform6D` and `orpy.IkParameterizationType.TranslationDirection5D` collision_free: bool, optional If true, find only collision-free solutions freeinc: float, optional The free increment (discretization) to be used for the free DOF when the target is 5D. Returns ------- solutions: list The list of IK solutions found """ # Populate the target list target_list = [] if iktype == orpy.IkParameterizationType.TranslationDirection5D: if type(target) is not orpy.Ray: ray = ru.conversions.to_ray(target) target_list.append(ray) else: target_list.append(target) elif iktype == orpy.IkParameterizationType.Transform6D: if type(target) is orpy.Ray: Tray = ru.conversions.from_ray(target) for angle in np.arange(0, 2 * np.pi, freeinc): Toffset = orpy.matrixFromAxisAngle(angle * br.Z_AXIS) target_list.append(np.dot(Tray, Toffset)) else: target_list.append(target) # Concatenate all the solutions solutions = [] for goal in target_list: ikparam = orpy.IkParameterization(goal, iktype) manipulator = robot.GetActiveManipulator() opt = 0 if collision_free: opt = orpy.IkFilterOptions.CheckEnvCollisions solutions += list(manipulator.FindIKSolutions(ikparam, opt)) return solutions
def main(): for i in range(1): """ Tz = r.matrixFromAxisAngle([0,0,numpy.pi/2]) Tz[0,3] = 0.4 Tz[1,3] = 1.6 Tz[2,3] = -0.01 """ Tz = r.matrixFromAxisAngle([0, 0, 0]) Tz[2, 3] = 0.01 runtrial(Tz, False, True, False)
def control_robot_with_the_keyboard(self): OFFSET = 0.07 print "" print "w: Move arm forward." print "s: Move arm backwards." print "a: Move arm left." print "d: Move arm right." print "q: Rotate end-effector anticlockwise." print "e: Rotate end-effector clockwise." print "o: Open end-effector." print "c: Close end-effector." print "" print "Type 'exit' to stop the demo and enter in IPython environment." while True: action = raw_input("Menu Action: ") if action == "exit": # EXIT Demo break if action == "q": # Rotate Anti-clockwise anticlockwise = matrixFromAxisAngle([0, 0, numpy.pi / 6]) self.rotate_hand(anticlockwise) elif action == "e": # Rotate Clockwise clockwise = matrixFromAxisAngle([0, 0, -numpy.pi / 6]) self.rotate_hand(clockwise) elif action == "o": # Open Gripper self.robot.open_gripper() elif action == "c": # Close Gripper self.robot.close_gripper() elif action in ["w", "s", "a", "d"]: x_offset = 0 y_offset = 0 if action == "w": x_offset = OFFSET # Move Forward if action == "s": x_offset = -OFFSET # Move Backwards if action == "a": y_offset = -OFFSET # Move Left if action == "d": y_offset = OFFSET # Move Right self.move_hand(x_offset, y_offset)
def test_load_blade(self): point_00 = array([-0.1521958 , 0.53814109, -3.76572666, 0.08927295, 0.16974092, -0.98143689]) T = matrixFromAxisAngle([pi / 4, 0, 0]) blade_00 = self.DB.load_blade() self.assertTrue(isinstance(blade_00.trajectories,list)) assert_allclose(blade_00.trajectories[0][0],point_00) point_45 = concatenate((dot(T[0:3, 0:3], blade_00.trajectories[0][0][0:3]), dot(T[0:3, 0:3], blade_00.trajectories[0][0][3:6]))) blade_45 = self.DB_45.load_blade() self.assertTrue(isinstance(blade_45.trajectories, list)) assert_allclose(blade_45.trajectories[0][0], point_45)
def rotate_point_cloud_tfm(cloud): A = np.cov(cloud.T) U, _, _ = np.linalg.svd(A) if U[2, -1] < 0: dir = -U[:, -1] else: dir = U[:, -1] center_xyz = np.median(cloud, axis=0) axis = np.cross(dir, [0,0,1]) angle = np.arccos(np.dot(dir, [0,0,1])) tfm = openravepy.matrixFromAxisAngle(axis, angle) return tfm
def rotate_point_cloud_tfm(cloud): A = np.cov(cloud.T) U, _, _ = np.linalg.svd(A) if U[2, -1] < 0: dir = -U[:, -1] else: dir = U[:, -1] center_xyz = np.median(cloud, axis=0) axis = np.cross(dir, [0, 0, 1]) angle = np.arccos(np.dot(dir, [0, 0, 1])) tfm = openravepy.matrixFromAxisAngle(axis, angle) return tfm
def Move(self, vel, execute=False, timeout=None, **kwargs): direction = numpy.array([1., 0., 0.]) with self.robot.GetEnv(): start_pose = self.robot.GetTransform() offset_pose = numpy.eye(4) offset_pose[0:3, 3] = vel[0] * direction transl_pose = numpy.dot(start_pose, offset_pose) relative_pose = openravepy.matrixFromAxisAngle([0., 0., vel[1]]) goal_pose = numpy.dot(transl_pose, relative_pose) traj = util.create_affine_trajectory(self.robot, [start_pose, goal_pose]) if (execute): self.robot.ExecutePath(traj, **kwargs) return traj
def ComposeAction(self, prevDesc, theta): '''Creates a new action and hand descriptor objects.''' action = [prevDesc.image, copy(theta)] R = openravepy.matrixFromAxisAngle(prevDesc.approach, theta[0])[0:3,0:3] approach = prevDesc.approach axis = dot(R, prevDesc.axis) center = prevDesc.center T = hand_descriptor.PoseFromApproachAxisCenter(approach, axis, center) desc = HandDescriptor(T) return action, desc
def extract_hitch(rgb, depth, T_w_k, dir=None, radius=0.016, length =0.215, height_range=[0.70,0.80]): """ template match to find the hitch in the picture, get the xyz at those points using the depth image, extend the points down to aid in tps. """ template_fname = osp.join(hd_data_dir, 'hitch_template.jpg') template = cv2.imread(template_fname) h,w,_ = template.shape res = cv2.matchTemplate(rgb, template, cv2.TM_CCORR_NORMED) _, _, _, max_loc = cv2.minMaxLoc(res) top_left = max_loc bottom_right = (top_left[0]+w, top_left[1]+h) if DEBUG_PLOTS: cv2.rectangle(rgb,top_left, bottom_right, (255,0,0), thickness=2) cv2.imshow("hitch detect", rgb) cv2.waitKey() # now get all points in a window around the hitch loc: xyz_k = clouds.depth_to_xyz(depth, asus_xtion_pro_f) xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:] hitch_pts = xyz_w[top_left[1]:bottom_right[1], top_left[0]:bottom_right[0],:] height_mask = (hitch_pts[:,:,2] > height_range[0]) & (hitch_pts[:,:,2] < height_range[1]) hitch_pts = hitch_pts[height_mask] ## add pts along the rod: center_xyz = np.median(hitch_pts, axis=0) ang = np.linspace(0, 2*np.pi, 30) circ_pts = radius*np.c_[np.cos(ang), np.sin(ang)] + center_xyz[None,:2] circ_zs = np.linspace(center_xyz[2], length+center_xyz[2], 30) rod_pts = np.empty((0,3)) for z in circ_zs: rod_pts = np.r_[rod_pts, np.c_[circ_pts, z*np.ones((len(circ_pts),1))] ] all_pts = np.r_[hitch_pts, rod_pts] if dir is None: return all_pts, center_xyz else: axis = np.cross([0,0,1], dir) angle = np.arccos(np.dot([0,0,1], dir)) tfm = matrixFromAxisAngle(axis, angle) tfm[:3,3] = - tfm[:3,:3].dot(center_xyz) + center_xyz return np.asarray([tfm[:3,:3].dot(point) + tfm[:3,3] for point in all_pts]), center_xyz
def Collides(self, config): x = config[0] y = config[1] theta = config[2] transform = matrixFromAxisAngle([0,0,theta]) transform[0][3] = x transform[1][3] = y # Assigns Transform to Robot and Checks Collision with self.herb.env: self.robot.SetTransform(transform) # time.sleep(0.01) if self.herb.env.CheckCollision(self.robot,self.table) == True: return True else: return False
def Rotate(self, angle_rad, execute=True, **kw_args): """ Rotates the robot the desired distance @param angle_rad the number of radians to rotate @param timeout duration to wait for execution """ with self.robot.GetEnv(): start_pose = self.robot.GetTransform() relative_pose = openravepy.matrixFromAxisAngle([ 0., 0., angle_rad ]) goal_pose = numpy.dot(start_pose, relative_pose) path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ]) if execute: return self.robot.ExecutePath(path, **kw_args) else: return path
def generate_trajectories(self, iter_surface, vis = None): """ Method generate the coating trajectories. The trajectories are the intersection between two surfaces: the blade model, and the surface to be iterated, e.g. spheres. The algorithm follows the marching method, documentation available in: http://www.mathematik.tu-darmstadt.de/~ehartmann/cdgen0104.pdf page 94 intersection surface - surface. This is a data-intensive computing and might freeze your computer. Args: iter_surface: (@ref IterSurface) surface to be iterated, as mathtools.sphere. Returns: (float[n][6]) list of points and normals, intersection between two surfaces. """ step = self.trajectory_step trajectories = deepcopy(self.trajectories) if not issubclass(iter_surface.__class__, mathtools.IterSurface): raise TypeError("Object is not a valid surface.") if self.model_iter_surface is not None: if iter_surface.name() != self.model_iter_surface.name(): raise TypeError("Object iter_surface must have same type of model_iter_surface.") if not self.models: raise IndexError("Object models is empty. Load or create a model before generate trajectories.") self._blade.SetTransform(eye(4)) self._blade.SetTransform(dot(self._blade.GetTransform(), matrixFromAxisAngle([0, -self.turbine.config.environment.blade_angle, 0])) ) point_on_surfaces = self._compute_initial_point(iter_surface, trajectories) self.trajectory_iter_surface = iter_surface while iter_surface.criteria(): model = self.select_model(point_on_surfaces) trajectories.append(self.draw_parallel(point_on_surfaces, model, iter_surface, step, vis)) p0 = trajectories[-1][-1] iter_surface.update() point_on_surfaces = mathtools.curvepoint(model, iter_surface, p0[0:3]) self.trajectories = trajectories return trajectories
def observe_cloud2(self, radius, axis_upsample=0, radius_sample=0, angle_sample=0): axis_pts = self.rope.GetControlPoints() indices = range(len(axis_pts)) if axis_upsample != 0: lengths = np.r_[0, self.rope.GetHalfHeights() * 2] summed_lengths = np.cumsum(lengths) assert len(lengths) == len(axis_pts) indices = np.interp( np.linspace(0, summed_lengths[-1], axis_upsample * len(lengths)), summed_lengths, indices) axis_pts = math_utils.interp2d( np.linspace(0, summed_lengths[-1], axis_upsample * len(lengths)), summed_lengths, axis_pts) half_heights = self.rope.GetHalfHeights() rotates = self.rope.GetRotations() translates = self.rope.GetTranslations() new_pts = [] for r in range(1, radius_sample + 1): d = r * radius / float(radius_sample) for a in range(angle_sample): local_rotate = matrixFromAxisAngle( [a / float(angle_sample) * 2 * np.pi, 0, 0])[:3, :3] for index in indices: node_id = np.min( [np.floor(index), len(self.rope.GetNodes()) - 1]) h = half_heights[node_id] v = np.array([-h + 2 * h * (index - node_id), d, 0]) rotate = rotates[node_id] translate = translates[node_id] v = rotate.dot(local_rotate.dot(v)) + translate new_pts.append(v) for pt in axis_pts: new_pts.append(pt) return np.array(new_pts)
def generate_random_pos(robot, obj_to_grasp = None): """Generate a random position for the robot within the boundaries of the world. """ T = robot.GetTransform() return T envmin, envmax = utils.get_environment_limits(robot.GetEnv(), robot) if obj_to_grasp is None: max_x = envmax[0] max_y = envmax[1] max_th = np.pi min_x = envmin[0] min_y = envmin[1] min_th = np.pi else: if type(obj_to_grasp) is openravepy.KinBody: obj_pos = obj_to_grasp.GetTransform()[:3,-1] else: obj_pos = obj_to_grasp max_x = min(obj_pos[0] + 1.2, envmax[0]) max_y = min(obj_pos[1] + 1.2, envmax[1]) min_x = max(obj_pos[0] - 1.2, envmin[0]) min_y = max(obj_pos[1] - 1.2, envmin[1]) #print "X ", (min_x, max_x), " Y: ", (min_y, max_y) x = np.random.uniform(min_x, max_x) y = np.random.uniform(min_y, max_y) z = T[2,3] if obj_to_grasp is None: th = np.random.uniform(min_th, max_th) else: robot_pos = T[:3,-1] facing_angle = np.arctan2(obj_pos[1] -y , obj_pos[0] - x) th = np.random.uniform(facing_angle - np.pi/4., facing_angle + np.pi/4.) #rotation T = openravepy.matrixFromAxisAngle([0,0,th]) #translation T[:, -1] = [x, y, z, 1] return T
def load_fake_data_segment(sim_env, demofile, fake_data_segment, fake_data_transform, set_robot_state=True, floating=False): fake_seg = demofile[fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(fake_data_transform[3:6]) hmat[:3, 3] = fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] r2r = None if not floating: r2r = ros2rave.RosToRave(sim_env.robot, asarray(fake_seg["joint_states"]["name"])) if set_robot_state: r2r.set_values(sim_env.robot, asarray(fake_seg["joint_states"]["position"][0])) return new_xyz, r2r
def test_closest_arm_pose(self): env = ParamSetup.setup_env() # env.SetViewer('qtcoin') can = ParamSetup.setup_blue_can() robot = ParamSetup.setup_pr2() can.pose = np.array([[0,-.2,.8]]).T can_body = OpenRAVEBody(env, can.name, can.geom) can_body.set_pose(can.pose.flatten(), can.rotation.flatten()) can_body.set_transparency(.7) robot.pose = np.array([[-.5,0,0]]).T robot_body = OpenRAVEBody(env, robot.name, robot.geom) robot_body.set_transparency(.7) robot_body.set_pose(robot.pose.flatten()) robot_body.set_dof(robot.backHeight, robot.lArmPose.flatten(), robot.lGripper, robot.rArmPose.flatten(), robot.rGripper) can_trans = OpenRAVEBody.transform_from_obj_pose(can.pose, can.rotation) rot_mat = matrixFromAxisAngle([0, np.pi/2, 0]) rot_mat = can_trans[:3, :3].dot(rot_mat[:3, :3]) can_trans[:3, :3] = rot_mat torso_pose, arm_pose = sampling.get_torso_arm_ik(robot_body, can_trans, robot.rArmPose) robot_body.set_dof(robot.backHeight, robot.lArmPose.flatten(), robot.lGripper, arm_pose, robot.rGripper)
def sample_ee_from_target(self, target): """ Sample all possible EE Pose that pr2 can grasp with target: parameter of type Target return: list of tuple in the format of (ee_pos, ee_rot) """ possible_ee_poses = [] targ_pos, targ_rot = target.value.flatten(), target.rotation.flatten() ee_pos = targ_pos.copy() target_trans = OpenRAVEBody.transform_from_obj_pose(targ_pos, targ_rot) # rotate can's local z-axis by the amount of linear spacing between 0 to 2pi angle_range = np.linspace(0, np.pi * 2, num=SAMPLE_SIZE) for rot in angle_range: target_trans = OpenRAVEBody.transform_from_obj_pose(targ_pos, targ_rot) # rotate new ee_pose around can's rotation axis rot_mat = matrixFromAxisAngle([0, 0, rot]) ee_trans = target_trans.dot(rot_mat) ee_rot = OpenRAVEBody.obj_pose_from_transform(ee_trans)[3:] possible_ee_poses.append((ee_pos, ee_rot)) return possible_ee_poses
def SetCameraFromXML(viewer, xml): if isinstance(viewer, openravepy.Environment): viewer = viewer.GetViewer() import lxml.etree from StringIO import StringIO padded_xml = '<bogus>{0:s}</bogus>'.format(xml) camera_xml = lxml.etree.parse(StringIO(padded_xml)) translation_raw = camera_xml.find('//camtrans').text axis_raw = camera_xml.find('//camrotationaxis').text focal_raw = camera_xml.find('//camfocal').text translation = numpy.loadtxt(StringIO(translation_raw)) axis_angle = numpy.loadtxt(StringIO(axis_raw)) axis_angle = axis_angle[3] * axis_angle[0:3] * (numpy.pi / 180) focal = float(focal_raw) transform = openravepy.matrixFromAxisAngle(axis_angle) transform[0:3, 3] = translation viewer.SetCamera(transform, focal)
def setUp(self): table_height = 0.77 helix_ang0 = 0 helix_ang1 = 4*np.pi helix_radius = .2 helix_center = np.r_[.6, 0] helix_height0 = table_height + .15 helix_height1 = table_height + .15 + .3 helix_length = np.linalg.norm(np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0]) num = np.round(helix_length/.02) helix_angs = np.linspace(helix_ang0, helix_ang1, num) helix_heights = np.linspace(helix_height0, helix_height1, num) init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights] rope_params = sim_util.RopeParams() cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.6, helix_radius, table_height + .25] cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35] sim_objs = [] sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False)) sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params)) sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True)) sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True)) self.sim = DynamicSimulation() self.sim.add_objects(sim_objs) # rotate cylinders by 90 deg for i in range(2): bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i) T = openravepy.matrixFromAxisAngle(np.array([np.pi/2, 0, 0])) T[:3,3] = bt_cyl.GetTransform()[:3, 3] bt_cyl.SetTransform(T) # SetTransform needs to be used in the Bullet object, not the openrave body self.sim.update() self.sim.add_objects([XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)]) self.sim.robot.SetDOFValues([0.25], [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(self.sim)
def calc_Thk(xyz, rod): T_h_k = openravepy.matrixFromAxisAngle(rod) T_h_k[:3,3] += xyz return T_h_k
def main(): import IPython demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 if args.execution: rospy.init_node("exec_task",disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) else: #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) print "got new xyz" redprint(new_xyz) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ ### Old end effector forces at r_gripper_tool_frame (including torques) old_forces = seg_info['end_effector_forces'][:,0:3,:] old_torques = seg_info['end_effector_forces'][:,3:6,:] redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) old_xyz_transformed = f.transform_points(old_xyz) #handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed]))) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'r': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj #print old_ee_traj old_ee_pos = old_ee_traj[:,0:3,3] #print old_ee_pos # End effector forces as oppossed to end effector trajectories dfdxs = f.compute_jacobian(old_ee_pos) old_eefs = [] new_eefs = [] for i in xrange(len(old_forces)): dfdx = np.matrix(dfdxs[i]) old_force = np.matrix(old_forces[i]) old_torque = np.matrix(old_torques[i]) new_force = (dfdx * old_force) new_torque = (dfdx * old_torque) old_eefs.append(np.vstack((old_force, old_torque))) new_eefs.append(np.vstack((new_force, new_torque))) old_eefs = np.asarray(old_eefs)[:,:,0] new_eefs = np.asarray(new_eefs)[:,:,0] force_data = {} force_data['old_eefs'] = old_eefs force_data['new_eefs'] = new_eefs force_file = open("trial.pickle", 'wa') pickle.dump(force_data, force_file) force_file.close() new_ee_traj_x = new_ee_traj miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution=="real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg)) # figure out how we're gonna resample stuff lr2oldtraj = {} for lr in 'r': manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1]) #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end #if arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE lr2oldtraj[lr] = old_joint_traj if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, lr2oldtraj['r'].shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) # Setting robot arm to joint trajectory r_vals = lr2oldtraj['r'][i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans #IPython.embed() f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 # FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time #### new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample if args.no_ds: new_eefs_segment_rs = new_eefs_segment else: new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment) ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] if args.no_ds: old_joint_traj_rs = old_joint_traj new_ee_traj_rs = new_ee_traj ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds) new_ee_traj_rs = new_ee_traj_rs[ds_inds] old_joint_traj_rs = old_joint_traj_rs[ds_inds] new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj) else: old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) if args.execution: Globals.pr2.update_rave() part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name]))) redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) redprint("Press enter to set gripper") raw_input() #for lr in 'r': # set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) if args.pid: if not args.fake_data_segment: # If running on PR2, add initial state as waypoint and rough interpolate # Add initial position (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.arm_joint_names) traj_length = bodypart2traj['rarm'].shape[0] complete_joint_traj = np.zeros((traj_length+1, 7)) # To include initial state as a way point complete_joint_traj[1:,:] = bodypart2traj['rarm'] complete_joint_traj[0,:] = arm_position bodypart2traj['rarm'] = complete_joint_traj # Add initial eff J = np.matrix(np.resize(np.array(robot_states.call_return_jacobian()), (6, 7))) # Jacobian eff = robot_states.compute_end_effector_force(J, arm_effort).T eff = np.array(eff)[0] init_force = eff[:3] complete_force_traj = np.zeros((traj_length+1, 3)) complete_force_traj[1:,:] = new_eefs_segment_rs complete_force_traj[0,:] = init_force else: complete_force_traj = new_eefs_segment_rs # SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES if args.no_ds: traj = bodypart2traj['rarm'] stamps = asarray(seg_info['stamps']) times = np.array([stamps[i_end] - stamps[i_start]]) F = complete_force_traj else: times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time F = mu.interp2d(times_up, times, complete_force_traj) #Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0])) if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, traj.shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1))) # Setting robot arm to joint trajectory r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() qs = np.array(np.matrix(traj).T) # 7 x n qs = np.resize(traj, (1, qs.shape[1]*7))[0] #resize the data to 1x7n (n being the number of steps) F = np.array(np.matrix(F).T) # 6 x n F = np.resize(F, (1, F.shape[1]*6))[0] #resize the data to 1x3n # Controller code allCs = [] x = np.ones(6) * 1 v = np.ones(6) * 1e-3 a = np.ones(6) * 1e-6 Ct = np.diag(np.hstack((x, v, a))) num_steps = i_end - i_start + 1 #need to figure out the stamps correctly stamps = asarray(seg_info['stamps'][i_start:i_end+1]) for t in range(num_steps-1, -1, -1): allCs.append(Ct) m = np.ones(6) Kps = [] Kvs = [] Ks = [] Qt = None Vs = [] for t in range(num_steps-1, -1, -1): if Qt is None: Qt = allCs[t] else: Ft = np.zeros((12, 18)) for j in range(12): Ft[j][j] = 1.0 deltat = abs(stamps[t+1] - stamps[t]) #print(deltat) for j in range(6): Ft[j][j+6] = deltat for j in range(6): Ft[j+6][j+12] = deltat/m[j] for j in range(6): Ft[j][j+12] = ((deltat)**2)/m[j] Qt = allCs[t] + (np.transpose(Ft).dot(Vs[num_steps-2-t]).dot(Ft)) Qxx = Qt[0:12, 0:12] Quu = Qt[12:18, 12:18] Qxu = Qt[0:12, 12:18] Qux = Qt[12:18, 0:12] Quuinv = np.linalg.inv(Quu) Vt = Qxx - Qxu.dot(Quuinv).dot(Qux) Kt = -1*(Quuinv.dot(Qux)) Ks.append(Kt) Kps.append(Kt[:, 0:6]) Kvs.append(Kt[:, 6:12]) Vs.append(Vt) Ks = Ks[::-1] Kps = Kps[::-1] Kvs = Kvs[::-1] JKpJ = [] JKvJ = [] toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0])) toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0])) JacobianOlds = asarray(seg_info["jacobians"][i_start:i_end+1]) for i in range(i_end- i_start + 1): J = JacobianOlds[i] psuedoJ = np.linalg.inv(np.transpose(J).dot(J)).dot(np.transpose(J)) #JKpJ.append(np.transpose(J).dot(Kps[i]).dot(J) + toAddJkpJ*0.001) #JKvJ.append(np.transpose(J).dot(Kvs[i]).dot(J) + toAddJkvJ*0.001) JKpJ.append(psuedoJ.dot(Kps[i]).dot(J) + toAddJkpJ*0.001) JKvJ.append(psuedoJ.dot(Kvs[i]).dot(J) + toAddJkvJ*0.001) if args.useJK: Kps = [] Kvs = [] for i in range(i_end- i_start + 1): Kps.append(np.zeros((6,6))) Kvs.append(np.zeros((6,6))) else: JKpJ = [] JKvJ = [] for i in range(i_end- i_start + 1): JKpJ.append(np.zeros((7,7))) JKvJ.append(np.zeros((7,7))) Kps = np.asarray(Kps) Kvs = np.asarray(Kvs) JKpJ = np.asarray(JKpJ) JKvJ = np.asarray(JKvJ) IPython.embed() # # Temp Kps and Kvs for testing """ toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0])) toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0])) length = complete_force_traj.shape[0] JKpJ = np.asarray([toAddJkpJ for i in range(length)]) JKpJ = np.resize(JKpJ, (1, 49*length))[0] JKvJ = np.asarray([toAddJkvJ for i in range(length)]) JKvJ = np.resize(JKvJ, (1, 49*length))[0] """ # [traj, Kp, Kv, F, use_force, seconds] Kps = np.resize(Kps, (1, 36 * num_steps))[0] Kvs = np.resize(Kvs, (1, 36 * num_steps))[0] JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0] JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0] data = np.zeros((1, len(qs) + len(JKpJ) + len(JKvJ) + len(F) + len(Kps) + len(Kvs) + 2)) data[0][0:len(qs)] = qs data[0][len(qs):len(qs)+len(JKpJ)] = JKpJ data[0][len(qs)+len(JKpJ):len(qs)+len(JKpJ)+len(JKvJ)] = JKvJ data[0][len(qs)+len(JKpJ)+len(JKvJ):len(qs)+len(JKpJ)+len(JKvJ)+len(F)] = F data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)] = Kps data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)+len(Kvs)] = Kvs data[0][-2] = args.use_force data[0][-1] = stamps[i_end] - stamps[i_start] msg = Float64MultiArray() msg.data = data[0].tolist() pub = rospy.Publisher("/controller_data", Float64MultiArray) redprint("Press enter to start trajectory") raw_input() time.sleep(1) pub.publish(msg) time.sleep(1) else: #if not success: break if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj)
def track_video_frames_online(video_dir, T_w_k, init_tfm, table_plane, ref_image = None, rope_params = None): video_stamps = np.loadtxt(osp.join(video_dir,demo_names.stamps_name)) env = openravepy.Environment() stdev = np.array([]) rgb = cv2.imread(osp.join(video_dir,demo_names.rgb_name%0)) rgb = color_match.match(ref_image, rgb) assert rgb is not None depth = cv2.imread(osp.join(video_dir,demo_names.depth_name%0), 2) assert depth is not None rope_xyz = extract_red(rgb, depth, T_w_k) table_dir = np.array([table_plane[0], table_plane[1], table_plane[2]]) table_dir = table_dir / np.linalg.norm(table_dir) table_dir = init_tfm[:3,:3].dot(table_dir) if np.dot([0,0,1], table_dir) < 0: table_dir = -table_dir table_axis = np.cross(table_dir, [0,0,1]) table_angle = np.arccos(np.dot([0,0,1], table_dir)) table_tfm = openravepy.matrixFromAxisAngle(table_axis, table_angle) table_center_xyz = np.mean(rope_xyz, axis=0) table_tfm[:3,3] = - table_tfm[:3,:3].dot(table_center_xyz) + table_center_xyz init_tfm = table_tfm.dot(init_tfm) tracked_nodes = None for (index, stamp) in zip(range(len(video_stamps)), video_stamps): print index, stamp rgb = cv2.imread(osp.join(video_dir,demo_names.rgb_name%index)) rgb = color_match.match(ref_image, rgb) assert rgb is not None depth = cv2.imread(osp.join(video_dir,demo_names.depth_name%index), 2) assert depth is not None color_cloud = extract_rope_tracking(rgb, depth, T_w_k) # color_cloud is at first in camera frame #print "cloud in camera frame" #print "===================================" #print color_cloud[:, :3] color_cloud = clouds.downsample_colored(color_cloud, .01) color_cloud[:,:3] = color_cloud[:,:3].dot(init_tfm[:3,:3].T) + init_tfm[:3,3][None,:] # color_cloud now is in global frame raw_color_cloud = np.array(color_cloud) ########################################## ### remove the shadow points on the desk ########################################## color_cloud = remove_shadow(color_cloud) if tracked_nodes is not None: color_cloud = rope_shape_filter(tracked_nodes, color_cloud) if index == 0: rope_xyz = extract_red(rgb, depth, T_w_k) rope_xyz = clouds.downsample(rope_xyz, .01) rope_xyz = rope_xyz.dot(init_tfm[:3, :3].T) + init_tfm[:3,3][None,:] rope_nodes = rope_initialization.find_path_through_point_cloud(rope_xyz) # rope_nodes and rope_xyz are in global frame # print rope_nodes table_height = rope_xyz[:,2].min() - .02 table_xml = make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) env.LoadData(table_xml) bulletsimpy.sim_params.scale = 10 bulletsimpy.sim_params.maxSubSteps = 200 if rope_params is None: rope_params = bulletsimpy.CapsuleRopeParams() rope_params.radius = 0.005 #angStiffness: a rope with a higher angular stiffness seems to have more resistance to bending. #orig self.rope_params.angStiffness = .1 rope_params.angStiffness = .1 #A higher angular damping causes the ropes joints to change angle slower. #This can cause the rope to be dragged at an angle by the arm in the air, instead of falling straight. #orig self.rope_params.angDamping = 1 rope_params.angDamping = 1 #orig self.rope_params.linDamping = .75 #Not sure what linear damping is, but it seems to limit the linear accelertion of centers of masses. rope_params.linDamping = .75 #Angular limit seems to be the minimum angle at which the rope joints can bend. #A higher angular limit increases the minimum radius of curvature of the rope. rope_params.angLimit = .4 #TODO--Find out what the linStopErp is #This could be the tolerance for error when the joint is at or near the joint limit rope_params.linStopErp = .2 bt_env = bulletsimpy.BulletEnvironment(env, []) bt_env.SetGravity([0,0,-0.1]) rope = bulletsimpy.CapsuleRope(bt_env, 'rope', rope_nodes, rope_params) continue #============================================================================== # rope_nodes = rope.GetNodes() # #print "rope nodes in camera frame" # R = init_tfm[:3,:3].T # t = - R.dot(init_tfm[:3,3]) # rope_in_camera_frame = rope_nodes.dot(R.T) + t[None,:] # #print rope_in_camera_frame # uvs = XYZ_to_xy(rope_in_camera_frame[:,0], rope_in_camera_frame[:,1], rope_in_camera_frame[:,2], asus_xtion_pro_f) # uvs = np.vstack(uvs) # #print uvs # #print "uvs" # uvs = uvs.astype(int) # # n_rope_nodes = len(rope_nodes) # # DEPTH_OCCLUSION_DIST = .03 # occ_dist = DEPTH_OCCLUSION_DIST # # vis = np.ones(n_rope_nodes) # # rope_depth_in_camera = np.array(rope_in_camera_frame) # depth_xyz = depth_to_xyz(depth, asus_xtion_pro_f) # # for i in range(n_rope_nodes): # u = uvs[0, i] # v = uvs[1, i] # # neighbor_radius = 10; # v_range = [max(0, v-neighbor_radius), v+neighbor_radius+1] # u_range = [max(0, u-neighbor_radius), u+neighbor_radius+1] # # xyzs = depth_xyz[v_range[0]:v_range[1], u_range[0]:u_range[1]] # # xyzs = np.reshape(xyzs, (xyzs.shape[0]*xyzs.shape[1], xyzs.shape[2])) # dists_to_origin = np.linalg.norm(xyzs, axis=1) # # dists_to_origin = dists_to_origin[np.isfinite(dists_to_origin)] # # #print dists_to_origin # min_dist_to_origin = np.min(dists_to_origin) # # print v, u, min_dist_to_origin, np.linalg.norm(rope_in_camera_frame[i]) # # if min_dist_to_origin + occ_dist > np.linalg.norm(rope_in_camera_frame[i]): # vis[i] = 1 # else: # vis[i] = 0 # # #print "vis result" # #print vis # # rope_depth_in_global = rope_depth_in_camera.dot(init_tfm[:3,:3].T) + init_tfm[:3,3][None,:] #============================================================================== depth_cloud = extract_cloud(depth, T_w_k) depth_cloud[:,:3] = depth_cloud[:,:3].dot(init_tfm[:3,:3].T) + init_tfm[:3,3][None,:] # depth_cloud now is in global frame [tracked_nodes, new_stdev] = bulletsimpy.py_tracking(rope, bt_env, init_tfm, color_cloud, rgb, depth, 5, stdev) stdev = new_stdev #print tracked_nodes #if index % 10 != 0: # continue print index xx, yy = np.mgrid[-1:3, -1:3] zz = np.ones(xx.shape) * table_height table_cloud = [xx, yy, zz] fig.clf() ax = fig.gca(projection='3d') ax.set_autoscale_on(False) print init_tfm[:,3] ax.plot(depth_cloud[:,0], depth_cloud[:,1], depth_cloud[:,2], 'go', alpha=0.1) ax.plot(color_cloud[:,0], color_cloud[:,1], color_cloud[:,2]-0.1, 'go') ax.plot(raw_color_cloud[:,0], raw_color_cloud[:,1], raw_color_cloud[:,2] -0.2, 'ro') #ax.plot(rope_depth_in_global[:,0], rope_depth_in_global[:,1], rope_depth_in_global[:,2], 'ro') ax.plot_surface(table_cloud[0], table_cloud[1], table_cloud[2], color = (0,1,0,0.5)) ax.plot(tracked_nodes[:,0], tracked_nodes[:,1], tracked_nodes[:,2], 'bo') ax.plot([init_tfm[0,3]], [init_tfm[1,3]], [init_tfm[2,3]], 'ro') fig.show() raw_input()
def main(): import IPython demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 if args.execution: rospy.init_node("exec_task",disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) else: #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) print "got new xyz" redprint(new_xyz) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ ### Old end effector forces at r_gripper_tool_frame (eliminating the torques for now) old_eefs = seg_info['end_effector_forces'][:,0:3,:] redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) old_xyz_transformed = f.transform_points(old_xyz) #handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed]))) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'r': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj #print old_ee_traj old_ee_pos = old_ee_traj[:,0:3,3] #print old_ee_pos # End effector forces as oppossed to end effector trajectories dfdxs = f.compute_jacobian(old_ee_pos) new_eefs = [] for i in xrange(len(old_eefs)): dfdx = np.matrix(dfdxs[i]) old_eef = np.matrix(old_eefs[i]) new_eefs.append(dfdx * old_eef) old_eefs = asarray(old_eefs)[:,:,0] new_eefs = asarray(new_eefs)[:,:,0] force_data = {} force_data['old_eefs'] = old_eefs force_data['new_eefs'] = new_eefs force_file = open("trial.pickle", 'wa') pickle.dump(force_data, force_file) force_file.close() new_ee_traj_x = new_ee_traj miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution=="real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg)) # figure out how we're gonna resample stuff lr2oldtraj = {} for lr in 'r': manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1]) #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end #if arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE lr2oldtraj[lr] = old_joint_traj if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, lr2oldtraj['r'].shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) # Setting robot arm to joint trajectory r_vals = lr2oldtraj['r'][i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(old_eefs[i])/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 # FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time #### new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample if args.no_ds: new_eefs_segment_rs = new_eefs_segment else: new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment) ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] if args.no_ds: old_joint_traj_rs = old_joint_traj new_ee_traj_rs = new_ee_traj ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds) new_ee_traj_rs = new_ee_traj_rs[ds_inds] old_joint_traj_rs = old_joint_traj_rs[ds_inds] new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj) else: old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) if args.execution: Globals.pr2.update_rave() part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name]))) redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) if args.pid: if not args.fake_data_segment: # If running on PR2, add initial state as waypoint and rough interpolate # Add initial position (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.arm_joint_names) traj_length = bodypart2traj['rarm'].shape[0] complete_joint_traj = np.zeros((traj_length+1, 7)) # To include initial state as a way point complete_joint_traj[1:,:] = bodypart2traj['rarm'] complete_joint_traj[0,:] = arm_position bodypart2traj['rarm'] = complete_joint_traj # Add initial eff J = np.matrix(np.resize(np.array(robot_states.call_return_jacobian()), (6, 7))) # Jacobian eff = robot_states.compute_end_effector_force(J, arm_effort).T eff = np.array(eff)[0] init_force = eff[:3] complete_force_traj = np.zeros((traj_length+1, 3)) complete_force_traj[1:,:] = new_eefs_segment_rs complete_force_traj[0,:] = init_force else: complete_force_traj = new_eefs_segment_rs # SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES if args.no_ds: traj = bodypart2traj['rarm'] stamps = asarray(seg_info['stamps']) times = np.array([stamps[i_end] - stamps[i_start]]) force = complete_force_traj else: times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time force = mu.interp2d(times_up, times, complete_force_traj) #Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0])) if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, traj.shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1))) # Setting robot arm to joint trajectory r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(force[i])/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() traj = np.array(np.matrix(traj).T) # 7 x n redprint(traj) traj = np.resize(traj, (1, traj.shape[1]*7)) #resize the data to 1x7n (n being the number of steps) force = np.array(np.matrix(force).T) # 3 x n force = np.resize(force, (1, force.shape[1]*3)) #resize the data to 1x3n #[traj, force, secs] traj_force_secs = np.zeros((1, traj.shape[1] + force.shape[1] + 2)) traj_force_secs[0,0:traj.shape[1]] = traj traj_force_secs[0,traj.shape[1]:traj.shape[1]+force.shape[1]] = force traj_force_secs[0,traj.shape[1]+force.shape[1]] = times[len(times)-1] traj_force_secs[0,traj.shape[1]+force.shape[1]+1] = args.use_force IPython.embed() msg = Float64MultiArray() msg.data = traj_force_secs[0].tolist() pub = rospy.Publisher("/joint_positions_forces_secs", Float64MultiArray) redprint("Press enter to start trajectory") raw_input() time.sleep(1) pub.publish(msg) time.sleep(1) else: #if not success: break if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj)
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 if args.execution: rospy.init_node("exec_task",disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) else: Globals.pr2.head.set_pan_tilt(0,1.2) Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() time.sleep(.5) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz,5, (1,0,0))) handles.append(Globals.env.plot3(new_xyz,5, (0,0,1))) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1))) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution=="real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg)) # figure out how we're gonna resample stuff lr2oldtraj = {} for lr in 'lr': manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1]) #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end if arm_moved(old_joint_traj): lr2oldtraj[lr] = old_joint_traj if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) #### ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) if args.execution: Globals.pr2.update_rave() new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj ################################ redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) for lr in 'lr': success &= set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) # Doesn't actually check if grab occurred, unfortunately if not success: break if len(bodypart2traj) > 0: success &= exec_traj_maybesim(bodypart2traj) if not success: break redprint("Segment %s result: %s"%(seg_name, success)) if args.fake_data_segment: break
cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.6, helix_radius, table_height + .25] cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35] sim_objs = [] sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False)) sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params)) sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True)) sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True)) sim = DynamicSimulation() sim.add_objects(sim_objs) sim.create_viewer() sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(sim) # rotate cylinders by 90 deg for i in range(2): bt_cyl = sim.bt_env.GetObjectByName('cyl%d'%i) T = openravepy.matrixFromAxisAngle(np.array([np.pi/2,0,0])) T[:3,3] = bt_cyl.GetTransform()[:3,3] bt_cyl.SetTransform(T) # SetTransform needs to be used in the Bullet object, not the openrave body sim.update() sim.settle(max_steps=1000) sim.viewer.Idle()
def calc_T(xyz, rod): Tt = rave.matrixFromAxisAngle(rod) Tt[:3, 3] = xyz return Tt
def main(): import IPython demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 if args.execution: rospy.init_node("exec_task",disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.eval.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.eval.fake_data_segment: fake_seg = demofile[args.eval.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.eval.fake_data_transform[3:6]) hmat[:3,3] = args.eval.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) else: #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) print "got new xyz" redprint(new_xyz) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ ### Old end effector forces at r_gripper_tool_frame (including torques) old_forces = seg_info['end_effector_forces'][:,0:3,:] old_torques = seg_info['end_effector_forces'][:,3:6,:] redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) old_xyz_transformed = f.transform_points(old_xyz) #handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed]))) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'r': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj #print old_ee_traj old_ee_pos = old_ee_traj[:,0:3,3] #print old_ee_pos # End effector forces as oppossed to end effector trajectories dfdxs = f.compute_jacobian(old_ee_pos) old_eefs = [] new_eefs = [] for i in xrange(len(old_forces)): dfdx = np.matrix(dfdxs[i]) old_force = np.matrix(old_forces[i]) old_torque = np.matrix(old_torques[i]) new_force = (dfdx * old_force) new_torque = (dfdx * old_torque) old_eefs.append(np.vstack((old_force, old_torque))) new_eefs.append(np.vstack((new_force, new_torque))) old_eefs = np.asarray(old_eefs)[:,:,0] new_eefs = np.asarray(new_eefs)[:,:,0] force_data = {} force_data['old_eefs'] = old_eefs force_data['new_eefs'] = new_eefs force_file = open("trial.pickle", 'wa') pickle.dump(force_data, force_file) force_file.close() new_ee_traj_x = new_ee_traj miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution=="real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg)) # figure out how we're gonna resample stuff lr2oldtraj = {} for lr in 'r': manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1]) #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end #if arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE lr2oldtraj[lr] = old_joint_traj if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, lr2oldtraj['r'].shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) # Setting robot arm to joint trajectory r_vals = lr2oldtraj['r'][i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans #IPython.embed() f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 # FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time #### new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample if args.no_ds: new_eefs_segment_rs = new_eefs_segment else: new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment) ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] if args.no_ds: old_joint_traj_rs = old_joint_traj new_ee_traj_rs = new_ee_traj ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds) new_ee_traj_rs = new_ee_traj_rs[ds_inds] old_joint_traj_rs = old_joint_traj_rs[ds_inds] new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj) else: old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs) if args.execution: Globals.pr2.update_rave() part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name]))) redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) #for lr in 'r': # set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) if args.pid: # Add trajectory to arrive at to initial state redprint("Press enter to use current position") raw_input() (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.r_arm_joint_names) diff = np.array(arm_position - bodypart2traj['rarm'][0,:]) maximum = max(abs(diff)) speed = (300.0/360.0*2*(np.pi)) time_needed = maximum / speed initial_pos_traj = mu.interp2d(np.arange(0, time_needed, 0.01), np.array([0,time_needed]), np.array([arm_position, bodypart2traj['rarm'][0,:]])) # length of the intial trajectory, use this length for padding PD gains initial_traj_length = initial_pos_traj.shape[0] initial_force_traj = np.array([np.zeros(6) for i in range(initial_traj_length)]) temptraj = bodypart2traj['rarm'] bodypart2traj['rarm'] = np.concatenate((initial_pos_traj, temptraj), axis=0) complete_force_traj = np.concatenate((initial_force_traj, new_eefs), axis=0) # SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES if args.no_ds: traj = bodypart2traj['rarm'] stamps = asarray(seg_info['stamps']) times = np.array([stamps[i_end] - stamps[i_start]]) F = complete_force_traj else: times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time F = mu.interp2d(times_up, times, complete_force_traj) #Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0])) if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, traj.shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1))) handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1))) # Setting robot arm to joint trajectory r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Idle() # Controller code in joint space traj_length = traj.shape[0] pgains = np.asarray([2400.0, 1200.0, 1000.0, 700.0, 300.0, 300.0, 300.0]) dgains = np.asarray([18.0, 10.0, 6.0, 4.0, 6.0, 4.0, 4.0]) m = np.array([3.33, 1.16, 0.1, 0.25, 0.133, 0.0727, 0.0727]) # masses in joint space (feed forward) vel_factor = 1e-3 #new costs covariances, means = analyze_data.run_analyze_data(args) CostsNew = [] counterCovs = 0 endTraj = False covThiss = [] for covMat in covariances: covThis = np.zeros((18,18)) covThis[0:6,0:6] = covMat[0:6,0:6] covThis[12:18,12:18] = covMat[6:12, 6:12] covThis[0:6, 12:18] = covMat[0:6, 6:12] covThis[12:18, 0:6] = covMat[6:12, 0:6] covThis[6:12, 6:12] = np.eye(6)*vel_factor covThis = np.diag(np.diag(covThis)) covThiss.append(covThis) #if len(covThiss) <= 69 and len(covThiss) >= 47: # covThis[12:18,12:18] = np.diag([0.13, 0.06, 0.07, 0.005, 0.01, 0.004]) for j in range(args.eval.downsample_traj): invCov = np.linalg.inv(covThis) CostsNew.append(invCov) counterCovs = counterCovs + 1 if counterCovs >= traj_length: endTraj = True break if endTraj: break allCs = [] x = np.ones(6) * 1 v = np.ones(6) * 1e-3 a = np.ones(6) * 1e-6 Ct = np.diag(np.hstack((x, v, a))) # in end effector space Ms = [] num_steps = i_end - i_start + 1 stamps = asarray(seg_info['stamps'][i_start:i_end+1]) arm = Globals.robot.GetManipulator("rightarm") jacobians = [] for i in range(traj.shape[0]): r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) jacobians.append(np.vstack((arm.CalculateJacobian(), arm.CalculateAngularVelocityJacobian()))) jacobians = asarray(jacobians) #jacobiansx = asarray(seg_info["jacobians"][i_start:i_end+1]) for t in range(num_steps): M = np.zeros((18, 21)) J = jacobians[t] M[0:6,0:7] = J M[6:12,7:14] = J mdiag = np.diag(m) # Convert joint feedforward values into diagonal array mdiag_inv = np.linalg.inv(mdiag) M[12:18,14:21] = np.linalg.inv((J.dot(mdiag_inv).dot(np.transpose(J)))).dot(J).dot(mdiag_inv) Ms.append(M) for t in range(num_steps): topad = np.zeros((21,21)) topad[0:7,0:7] = np.diag(pgains) * 0.1 topad[7:14,7:14] = np.diag(dgains) * 0.1 topad[14:21,14:21] = np.eye(7) * 0.1 #allCs.append(np.transpose(Ms[t]).dot(Ct).dot(Ms[t]) + topad) allCs.append(np.transpose(Ms[t]).dot(CostsNew[t]).dot(Ms[t]) + topad) Kps = [] Kvs = [] Ks = [] Qt = None Vs = [] for t in range(num_steps-1, -1, -1): if Qt is None: Qt = allCs[t] else: Ft = np.zeros((14, 21)) for j in range(14): Ft[j][j] = 1.0 deltat = abs(stamps[t+1] - stamps[t]) #print(deltat) for j in range(7): Ft[j][j+7] = deltat for j in range(7): Ft[j+7][j+14] = deltat/m[j] for j in range(7): Ft[j][j+14] = ((deltat)**2)/m[j] Qt = allCs[t] + (np.transpose(Ft).dot(Vs[num_steps-2-t]).dot(Ft)) Qxx = Qt[0:14, 0:14] Quu = Qt[14:21, 14:21] Qxu = Qt[0:14, 14:21] Qux = Qt[14:21, 0:14] Quuinv = np.linalg.inv(Quu) Vt = Qxx - Qxu.dot(Quuinv).dot(Qux) Vt = 0.5*(Vt + np.transpose(Vt)) Kt = -1*(Quuinv.dot(Qux)) Ks.append(Kt) Kps.append(Kt[:, 0:7]) Kvs.append(Kt[:, 7:14]) Vs.append(Vt) Ks = Ks[::-1] Kps = Kps[::-1] Kvs = Kvs[::-1] JKpJ = np.asarray(Kps) JKvJ = np.asarray(Kvs) total_length = num_steps + initial_traj_length # Pad initial traj with PD gains pgainsdiag = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0])) dgainsdiag = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0])) addkp = np.asarray([pgainsdiag for i in range(initial_traj_length)]) addkv = np.asarray([dgainsdiag for i in range(initial_traj_length)]) JKpJ = np.concatenate((addkp, JKpJ), axis=0) JKvJ = np.concatenate((addkv, JKvJ), axis=0) Kps = [] Kvs = [] for i in range(num_steps + initial_traj_length): Kps.append(np.zeros((6,6))) Kvs.append(np.zeros((6,6))) Kps = np.asarray(Kps) Kvs = np.asarray(Kvs) # Gains as JKpJ and JKvJ for testing if args.pdgains: JKpJ = np.asarray([pgainsdiag for i in range(total_length)]) JKvJ = np.asarray([dgainsdiag for i in range(total_length)]) qs = traj IPython.embed() Kps = np.resize(Kps, (1, 36 * total_length))[0] Kvs = np.resize(Kvs, (1, 36 * total_length))[0] JKvJ = np.resize(JKvJ, (1, 49*total_length))[0] JKpJ = np.resize(JKpJ, (1, 49*total_length))[0] # For use with new controller qs = np.resize(qs, (1, qs.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps) F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x6n # [traj, Kp, Kv, F, use_force, seconds] data = np.zeros((1, len(qs) + len(JKpJ) + len(JKvJ) + len(F) + len(Kps) + len(Kvs) + 2)) data[0][0:len(qs)] = qs data[0][len(qs):len(qs)+len(JKpJ)] = JKpJ data[0][len(qs)+len(JKpJ):len(qs)+len(JKpJ)+len(JKvJ)] = JKvJ data[0][len(qs)+len(JKpJ)+len(JKvJ):len(qs)+len(JKpJ)+len(JKvJ)+len(F)] = F data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)] = Kps data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)+len(Kvs)] = Kvs data[0][-2] = args.use_force data[0][-1] = stamps[i_end] - stamps[i_start] + (initial_traj_length * 0.01) # For use with old controller """ qs = np.array(np.matrix(traj).T) # 7 x n qs = np.resize(qs, (1, qs.shape[1]*7))[0] #resize the data to 1x7n (n being the number of steps) F = np.array(np.matrix(F).T) # 6 x n F = F[0:3,:] F = np.resize(F, (1, F.shape[1]*3))[0] #resize the data to 1x3n data = np.zeros((1, len(qs) + len(F) + 2)) data[0][0:len(qs)] = qs data[0][len(qs):len(qs)+len(F)] = F data[0][-1] = args.use_force data[0][-2] = stamps[i_end] - stamps[i_start] """ msg = Float64MultiArray() msg.data = data[0].tolist() pub = rospy.Publisher("/controller_data", Float64MultiArray) redprint("Press enter to start trajectory") raw_input() time.sleep(1) pub.publish(msg) time.sleep(1) else: #if not success: break if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj)