def GetHardGoal(params, yname): goal = params[yname+'_goal_arm_config'] base_goal = [np.array(params[yname+'_goal_base_pose'])[0,3], np.array(params[yname+'_goal_base_pose'])[1,3], orpy.axisAngleFromRotationMatrix(np.array(params[yname+'_goal_base_pose'])[:3,:3])[2]] goal.extend(base_goal) return goal
def GetHardGoal(self, params, yname): goal = params[yname+'_goal_arm_config'] base_goal = [np.array(params[yname+'_goal_base_pose'])[0,3], np.array(params[yname+'_goal_base_pose'])[1,3], orpy.axisAngleFromRotationMatrix(np.array(params[yname+'_goal_base_pose'])[:3,:3])[2]] goal.extend(base_goal) return goal
def get_angles(hmats): """ returns the angles of a list of 4x4 transformation matrices """ assert hmats.ndim ==3 and hmats.shape[1:]==(4,4) angles = np.empty(hmats.shape[0]) for i in xrange(hmats.shape[0]): angles[i] = np.linalg.norm(rave.axisAngleFromRotationMatrix(hmats[i,0:3,0:3])) return angles
def get_angles(hmats): """ returns the angles of a list of 4x4 transformation matrices """ assert hmats.ndim == 3 and hmats.shape[1:] == (4, 4) angles = np.empty(hmats.shape[0]) for i in xrange(hmats.shape[0]): angles[i] = np.linalg.norm( rave.axisAngleFromRotationMatrix(hmats[i, 0:3, 0:3])) return angles
def move_straight(self, dist): #TODO Fill in, remove sleep #time.sleep(0) with self.env: T1 = self.robot.GetTransform() # Get the current Transform T2 = openravepy.axisAngleFromRotationMatrix(T1[0:3,0:3]) # Get the current axis angles of the robot T1[0,3] = dist*np.cos(T2[2]) # Distance movied in X-direction T1[1,3] = dist*np.sin(T2[2]) # Distance moved in Y-direction self.robot.SetTransform(T1) # Setting the new transform
def rpy2axang(rpy): """ Converts a matrix of rpy (nx3) into a matrix of axis-angles (nx3). """ n = rpy.shape[0] assert rpy.shape[1]==3, "unknown shape." ax_ang = np.empty((n,3)) for i in xrange(n): th = rpy[i,:] ax_ang[i,:] = rave.axisAngleFromRotationMatrix(tfms.euler_matrix(th[0], th[1], th[2])) return ax_ang
def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1): goal = np.array(goal) if goal.ndim == 2: # assume a 4x4 transformation matrix angle = openravepy.axisAngleFromRotationMatrix(goal)[-1] goal = np.array([goal[0, -1], goal[1, -1], angle]) # find the boundaries of the environment envmin, envmax = utils.get_environment_limits(self.env, self.robot) with self.env: self.robot.SetAffineTranslationLimits(envmin, envmax) self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3]) self.robot.SetAffineRotationAxisMaxVels(np.ones(4)) self.robot.SetActiveDOFs( [], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1] ) # draw the marker if draw_marker: center = np.r_[goal[0:2], 0.2] xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0)) yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0)) h = self.env.drawlinelist( np.transpose( np.c_[ center - xaxis, center + xaxis, center - yaxis, center + yaxis, center + xaxis, center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis, center + 0.5 * xaxis - 0.5 * yaxis, ] ), linewidth=5.0, colors=np.array((0, 1, 0)), ) openravepy.RaveLogInfo("Planning to goal " + str(goal)) res = self.basemanip.MoveActiveJoints( goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True ) if res is None: raise ValueError("Could not find a trajectory") if execute: openravepy.RaveLogInfo("Waiting for controller to finish") self.robot.WaitForController(0) self.robot.GetController().Reset() return res
def GeodesicError(t1, t2): ''' Computes the error in global coordinates between two transforms @param t1 current transform @param t2 goal transform @return a 4-vector of [dx, dy, dz, solid angle] ''' trel = numpy.dot(numpy.linalg.inv(t1), t2) trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3]) omega = openravepy.axisAngleFromRotationMatrix(trel[0:3, 0:3]) angle = numpy.linalg.norm(omega) return numpy.hstack((trans, angle))
def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1): goal = np.array(goal) if goal.ndim == 2: #assume a 4x4 transformation matrix angle = openravepy.axisAngleFromRotationMatrix(goal)[-1] goal = np.array([goal[0, -1], goal[1, -1], angle]) # find the boundaries of the environment envmin, envmax = utils.get_environment_limits(self.env, self.robot) with self.env: self.robot.SetAffineTranslationLimits(envmin, envmax) self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3]) self.robot.SetAffineRotationAxisMaxVels(np.ones(4)) self.robot.SetActiveDOFs( [], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1]) # draw the marker if draw_marker: center = np.r_[goal[0:2], 0.2] xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0)) yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0)) h = self.env.drawlinelist(np.transpose( np.c_[center - xaxis, center + xaxis, center - yaxis, center + yaxis, center + xaxis, center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis, center + 0.5 * xaxis - 0.5 * yaxis]), linewidth=5.0, colors=np.array((0, 1, 0))) openravepy.RaveLogInfo("Planning to goal " + str(goal)) res = self.basemanip.MoveActiveJoints(goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True) if res is None: raise ValueError("Could not find a trajectory") if execute: openravepy.RaveLogInfo("Waiting for controller to finish") self.robot.WaitForController(0) self.robot.GetController().Reset() return res
def GeodesicTwist(t1, t2): ''' Computes the twist in global coordinates that corresponds to the gradient of the geodesic distance between two transforms. @param t1 current transform @param t2 goal transform @return twist in se(3) ''' trel = numpy.dot(numpy.linalg.inv(t1), t2) trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3]) omega = numpy.dot(t1[0:3, 0:3], openravepy.axisAngleFromRotationMatrix( trel[0:3, 0:3])) return numpy.hstack((trans, omega))
def test_drive_base(pose_targ): wxyz_targ = pose_targ[:4] xyz_targ = pose_targ[4:] robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05])) robot.SetActiveDOFs(np.r_[robot.GetManipulator("rightarm").GetArmIndices(), robot.GetLink("torso_lift_link").GetIndex()], rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1]) angle_init = np.random.rand() * 2*np.pi x_init = xyz_targ[0] - .5*np.cos(angle_init) y_init = xyz_targ[1] - .5*np.sin(angle_init) dofvals_init = np.r_[np.zeros(7), 0, x_init, y_init, angle_init] #start = robot. start = robot.GetActiveDOFValues(); end = dofvals_init n_steps = 10; init_traj = mu.linspace2d(start, end, n_steps) manipname = "active" prob_desc = make_request_skeleton(manipname, n_steps); prob_desc["basic_info"]["start_fixed"] = True prob_desc["costs"].append( {"type" : "pose", "name" : "pose", "params" : { "pos_coeffs" : [10,10,10], "rot_coeffs" : [0,0,0], "xyz" : xyz_targ, "wxyz" : wxyz_targ, "link" : "r_gripper_tool_frame", }}) prob_desc["init_info"]["type"] = "given_traj" angle = np.linalg.norm(rave.axisAngleFromRotationMatrix(robot.GetTransform())) prob_desc["init_info"]["data"] = [row.tolist() for row in init_traj] s = json.dumps(prob_desc) traj.SetInteractive(True); prob = traj.ConstructProblem(s, env) return traj.OptimizeProblem(prob)
def __init__(self, robot): self.robot = robot for joint in robot.GetJoints(): lower, upper = joint.GetLimits() lower = np.clip(lower, -np.pi, np.inf) upper = np.clip(upper, -np.inf, np.pi) self.add_trait(joint.GetName(), tr.Range(lower[0].item(), upper[0].item(), joint.GetValues()[0])) T = robot.GetTransform() rx,ry,rz = rave.axisAngleFromRotationMatrix(T[:3,:3]) tx,ty,tz = T[:3,3] self.add_trait("tftx", tr.Range(-5.,5,tx.item())) self.add_trait("tfty", tr.Range(-5.,5,ty.item())) self.add_trait("tftz", tr.Range(-5.,5,tz.item())) self.add_trait("tfrx", tr.Range(-5.,5,rx.item())) self.add_trait("tfry", tr.Range(-5.,5,ry.item())) self.add_trait("tfrz", tr.Range(-5.,5,rz.item())) self.on_trait_change(self.update_robot, 'anytrait') tr.HasTraits.__init__(self)
def __init__(self, robot): self.robot = robot for joint in robot.GetJoints(): lower, upper = joint.GetLimits() lower = np.clip(lower, -np.pi, np.inf) upper = np.clip(upper, -np.inf, np.pi) self.add_trait(joint.GetName(), tr.Range(lower[0].item(), upper[0].item(), joint.GetValues()[0])) T = robot.GetTransform() rx,ry,rz = rave.axisAngleFromRotationMatrix(T[:3,:3]) tx,ty,tz = T[:3,3] self.add_trait("tftx", tr.Range(-5.,5,tx.item())) self.add_trait("tfty", tr.Range(-5.,5,ty.item())) self.add_trait("tftz", tr.Range(-5.,5,tz.item())) self.add_trait("tfrx", tr.Range(-5.,5,rx.item())) self.add_trait("tfry", tr.Range(-5.,5,ry.item())) self.add_trait("tfrz", tr.Range(-5.,5,rz.item())) self.on_trait_change(self.update_robot, 'anytrait') tr.HasTraits.__init__(self)
def mat_to_base_pose(mat): pose = poseFromMatrix(mat) x = pose[4] y = pose[5] rot = axisAngleFromRotationMatrix(mat)[2] return np.array([x,y,rot])
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None, nullspace=JointLimitAvoidance, timelimit=5.0, step_size=0.001, position_tolerance=0.01, angular_tolerance=0.15, **kw_args): """ Plan to a desired end-effector offset with move-hand-straight constraint. movement less than distance will return failure. The motion will not move further than max_distance. @param robot @param direction unit vector in the direction of motion @param distance minimum distance in meters @param max_distance maximum distance in meters @param timelimit timeout in seconds @param stepsize step size in meters for the Jacobian pseudoinverse controller @param position_tolerance constraint tolerance in meters @param angular_tolerance constraint tolerance in radians @return traj """ if distance < 0: raise ValueError('Distance must be non-negative.') elif numpy.linalg.norm(direction) == 0: raise ValueError('Direction must be non-zero') elif max_distance is not None and max_distance < distance: raise ValueError('Max distance is less than minimum distance.') elif step_size <= 0: raise ValueError('Step size must be positive.') elif position_tolerance < 0: raise ValueError('Position tolerance must be non-negative.') elif angular_tolerance < 0: raise ValueError('Angular tolerance must be non-negative.') # save all active bodies so we only check collision with those active_bodies = [] for body in self.env.GetBodies(): if body.IsEnabled(): active_bodies.append(body) # Normalize the direction vector. direction = numpy.array(direction, dtype='float') direction /= numpy.linalg.norm(direction) # Default to moving an exact distance. if max_distance is None: max_distance = distance with robot: manip = robot.GetActiveManipulator() traj = openravepy.RaveCreateTrajectory(self.env, '') traj.Init(manip.GetArmConfigurationSpecification()) active_dof_indices = manip.GetArmIndices() limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices) initial_pose = manip.GetEndEffectorTransform() q = robot.GetDOFValues(active_dof_indices) traj.Insert(0, q) start_time = time.time() current_distance = 0.0 sign_flipper = 1 last_rot_error = 9999999999.0 try: while current_distance < max_distance: # Check for a timeout. current_time = time.time() if timelimit is not None and current_time - start_time > timelimit: raise PlanningError('Reached time limit.') # Compute joint velocities using the Jacobian pseudoinverse. q_dot = self.GetStraightVelocity(manip, direction, initial_pose, nullspace, step_size, sign_flipper=sign_flipper) q += q_dot robot.SetDOFValues(q, active_dof_indices) # Check for collisions. #if self.env.CheckCollision(robot): for body in active_bodies: if self.env.CheckCollision(robot, body): raise PlanningError('Encountered collision.') if robot.CheckSelfCollision(): raise PlanningError('Encountered self-collision.') # Check for joint limits. elif not (limits_lower < q).all() or not (q < limits_upper).all(): raise PlanningError('Encountered joint limit during Jacobian move.') # Check our distance from the constraint. current_pose = manip.GetEndEffectorTransform() a = initial_pose[0:3, 3] p = current_pose[0:3, 3] orthogonal_proj = (a - p) - numpy.dot(a - p, direction) * direction if numpy.linalg.norm(orthogonal_proj) > position_tolerance: raise PlanningError('Deviated from a straight line constraint.') # Check our orientation against the constraint. offset_pose = numpy.dot(numpy.linalg.inv(current_pose), initial_pose) offset_angle = openravepy.axisAngleFromRotationMatrix(offset_pose) offset_angle_norm = numpy.linalg.norm(offset_angle) if offset_angle_norm > last_rot_error + 0.0005: sign_flipper *= -1 last_rot_error = offset_angle_norm if offset_angle_norm > angular_tolerance: raise PlanningError('Deviated from orientation constraint.') traj.Insert(traj.GetNumWaypoints(), q) # Check if we've exceeded the maximum distance by projecting our # displacement along the direction. hand_pose = manip.GetEndEffectorTransform() displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3] current_distance = numpy.dot(displacement, direction) except PlanningError as e: # Throw an error if we haven't reached the minimum distance. if current_distance < distance: raise # Otherwise we'll gracefully terminate. else: logger.warning('Terminated early at distance %f < %f: %s', current_distance, max_distance, e.message) SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True) return traj
def vec2args(x): return x[0:3], x[3:6] def args2vec(xyz, rod): out = np.empty(6) out[0:3] = xyz out[3:6] = rod return out T_init = np.eye(4) xyz_init = T_init[:3, 3] rod_init = rave.axisAngleFromRotationMatrix(T_init[:3, :3]) print "> optimizing..." soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init), xtol=1e-2, ftol=1e-1, callback=plot_point) print "\t optimization done." (best_xyz, best_rod) = vec2args(soln) print "xyz, rod:", best_xyz, best_rod T_best = calc_T(best_xyz, best_rod) print "T_h_k:", T_best ## final display: pc2 = (np.c_[pc2, np.ones((pc2.shape[0], 1))].dot(T_best.T))[:, :3] clearreq = gen_mlab_request(mlab.clf) plotter.request(clearreq) c1req = gen_mlab_request(mlab.points3d, pc1[:, 0], pc1[:, 1], pc1[:, 2], color=(1, 0, 0), scale_factor=0.001)
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None, nullspace=JointLimitAvoidance, timelimit=5.0, step_size=0.001, position_tolerance=0.01, angular_tolerance=0.15, **kw_args): """ Plan to a desired end-effector offset with move-hand-straight constraint. movement less than distance will return failure. The motion will not move further than max_distance. @param robot @param direction unit vector in the direction of motion @param distance minimum distance in meters @param max_distance maximum distance in meters @param timelimit timeout in seconds @param stepsize step size in meters for the Jacobian pseudoinverse controller @param position_tolerance constraint tolerance in meters @param angular_tolerance constraint tolerance in radians @return traj """ if distance < 0: raise ValueError('Distance must be non-negative.') elif numpy.linalg.norm(direction) == 0: raise ValueError('Direction must be non-zero') elif max_distance is not None and max_distance < distance: raise ValueError('Max distance is less than minimum distance.') elif step_size <= 0: raise ValueError('Step size must be positive.') elif position_tolerance < 0: raise ValueError('Position tolerance must be non-negative.') elif angular_tolerance < 0: raise ValueError('Angular tolerance must be non-negative.') # save all active bodies so we only check collision with those active_bodies = [] for body in self.env.GetBodies(): if body.IsEnabled(): active_bodies.append(body) # Normalize the direction vector. direction = numpy.array(direction, dtype='float') direction /= numpy.linalg.norm(direction) # Default to moving an exact distance. if max_distance is None: max_distance = distance with robot: manip = robot.GetActiveManipulator() traj = openravepy.RaveCreateTrajectory(self.env, '') traj.Init(manip.GetArmConfigurationSpecification()) active_dof_indices = manip.GetArmIndices() limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices) initial_pose = manip.GetEndEffectorTransform() q = robot.GetDOFValues(active_dof_indices) traj.Insert(0, q) start_time = time.time() current_distance = 0.0 sign_flipper = 1 last_rot_error = 9999999999.0 try: while current_distance < max_distance: # Check for a timeout. current_time = time.time() if timelimit is not None and current_time - start_time > timelimit: raise PlanningError('Reached time limit.') # Compute joint velocities using the Jacobian pseudoinverse. q_dot = self.GetStraightVelocity(manip, direction, initial_pose, nullspace, step_size, sign_flipper=sign_flipper) q += q_dot robot.SetDOFValues(q, active_dof_indices) # Check for collisions. #if self.env.CheckCollision(robot): for body in active_bodies: if self.env.CheckCollision(robot, body): raise PlanningError('Encountered collision.') if robot.CheckSelfCollision(): raise PlanningError('Encountered self-collision.') # Check for joint limits. elif not (limits_lower < q).all() or not (q < limits_upper).all(): raise PlanningError( 'Encountered joint limit during Jacobian move.') # Check our distance from the constraint. current_pose = manip.GetEndEffectorTransform() a = initial_pose[0:3, 3] p = current_pose[0:3, 3] orthogonal_proj = ( a - p) - numpy.dot(a - p, direction) * direction if numpy.linalg.norm(orthogonal_proj) > position_tolerance: raise PlanningError( 'Deviated from a straight line constraint.') # Check our orientation against the constraint. offset_pose = numpy.dot(numpy.linalg.inv(current_pose), initial_pose) offset_angle = openravepy.axisAngleFromRotationMatrix( offset_pose) offset_angle_norm = numpy.linalg.norm(offset_angle) if offset_angle_norm > last_rot_error + 0.0005: sign_flipper *= -1 last_rot_error = offset_angle_norm if offset_angle_norm > angular_tolerance: raise PlanningError( 'Deviated from orientation constraint.') traj.Insert(traj.GetNumWaypoints(), q) # Check if we've exceeded the maximum distance by projecting our # displacement along the direction. hand_pose = manip.GetEndEffectorTransform() displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3] current_distance = numpy.dot(displacement, direction) except PlanningError as e: # Throw an error if we haven't reached the minimum distance. if current_distance < distance: raise # Otherwise we'll gracefully terminate. else: logger.warning('Terminated early at distance %f < %f: %s', current_distance, max_distance, e.message) SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True) return traj
def mat_to_base_pose(mat): pose = openravepy.poseFromMatrix(mat) x = pose[4] y = pose[5] rot = openravepy.axisAngleFromRotationMatrix(mat)[2] return x, y, rot
def mat_to_base_pose(mat): pose = poseFromMatrix(mat) x = pose[4] y = pose[5] rot = axisAngleFromRotationMatrix(mat)[2] return np.array([x, y, rot])
def axis_angle_from_rot(rot): return axisAngleFromRotationMatrix(rot)
def GetBasePoseForObjectGrasp(self, obj): # Load grasp database self.gmodel = openravepy.databases.grasping.GraspingModel( self.robot, obj) if not self.gmodel.load(): self.gmodel.autogenerate() base_pose = None grasp_config = None ################################################################### # TODO: Here you will fill in the function to compute # a base pose and associated grasp config for the # grasping the bottle ################################################################### #get the ordered valid grasp from homework1 print "robot start transformation -----------------" print self.robot.GetTransform() self.graspindices = self.gmodel.graspindices self.grasps = self.gmodel.grasps self.order_grasps() # get the grasp transform Tgrasp = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[10], collisionfree=True) # load inverserechability database irmodel = openravepy.databases.inversereachability.InverseReachabilityModel( robot=self.robot) starttime = time.time() print 'loading irmodel' if not irmodel.load(): irmodel.autogenerate() loaded = irmodel.load() print "irmodel loaded? {}".format(loaded) densityfn, samplerfn, bounds = irmodel.computeBaseDistribution(Tgrasp) #find the valid pose and joint states # initialize sampling parameters goals = [] numfailures = 0 N = 3 with self.robot.GetEnv(): while len(goals) < N: poses, jointstate = samplerfn(N - len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.manip.CheckIndependentCollision( openravepy.CollisionReport()): q = self.manip.FindIKSolution( Tgrasp, filteroptions=IkFilterOptions.CheckEnvCollisions) if q is not None: values = self.robot.GetDOFValues() values[self.manip.GetArmIndices()] = q goals.append((Tgrasp, pose, values)) elif self.manip.FindIKSolution(Tgrasp, 0) is None: numfailures += 1 # To do still #base_pose = goals[0][1] #grasp_config = goals[0][2] for i, goal in enumerate(goals): grasp_with_pose, pose, values = goal self.robot.SetTransform(pose) self.robot.SetJointValues(values) trans_pose = self.robot.GetTransform() angle_pose = openravepy.axisAngleFromRotationMatrix(trans_pose) pose = [trans_pose[0, 3], trans_pose[1, 3], angle_pose[2]] base_pose = numpy.array(pose) grasp_config = q #import IPython #IPython.embed() print "grasping result" print base_pose print grasp_config return base_pose, grasp_config
def tf_to_pose(tf): return np.r_[tf[:3,3], rave.axisAngleFromRotationMatrix(tf[:3,:3])]
def GetCurrentConfiguration(self): t = self.robot.GetTransform() aa = openravepy.axisAngleFromRotationMatrix(t) pose = [t[0,3], t[1,3], aa[2]] return numpy.array(pose)
#o = T_w_k[:3,3] #x = T_w_k[:3,0] #y = T_w_k[:3,1] #z = T_w_k[:3,2] #handles = [] #handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1))) #handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1))) #handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1))) #viewer.Idle() T_h_k_init = berkeley_pr2.T_h_k xyz_init = T_h_k_init[:3,3] rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3,:3]) f_init = 525 #from rapprentice.math_utils import linspace2d #sim_depths = calculate_sim_depths(xyz_init, rod_init, f_init) #cv2.imshow('hi',np.clip(sim_depths[0]/3,0,1)) #cv2.waitKey() #plot_real_and_sim(args2vec(xyz_init, rod_init, f_init)) #calculate_depth_error(xyz_init, rod_init, f_init) soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init, f_init),callback=plot_real_and_sim) (best_xyz, best_rod, best_f) = vec2args(soln) print "xyz, rod:", best_xyz, best_rod print "T_h_k:", calc_Thk(best_xyz, best_rod)
def GetBasePoseForObjectGrasp(self, obj): # Load grasp database graspModel = GraspModel(self.robot, obj) graspModel.order_grasps() for i in range(6): print 'Showing grasp ', i graspModel.show_grasp(graspModel.grasps_ordered[i], delay=self.delay) print "using grasp 0 to generate Transform " Tgrasp = graspModel.gmodel.getGlobalGraspTransform(graspModel.grasps_ordered[i], collisionfree=True) # get the grasp transform if Tgrasp != None: break base_pose = None grasp_config = None ################################################################### # TODO: Here you will fill in the function to compute # a base pose and associated grasp config for the # grasping the bottle ################################################################### self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(self.robot) loaded = self.irmodel.load() if loaded: densityfn,samplerfn,bounds = self.irmodel.computeBaseDistribution(Tgrasp) # densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities # samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states # bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]] goals = [] numfailures = 0 starttime = time.time() timeout = 5000 N = 5 with self.robot: while len(goals) < N: if time.time()-starttime > timeout: break poses,jointstate = samplerfn(N-len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.irmodel.manip.CheckIndependentCollision(openravepy.CollisionReport()): q = self.irmodel.manip.FindIKSolution(Tgrasp,filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions) if q is not None: values = self.robot.GetDOFValues() values[self.irmodel.manip.GetArmIndices()] = q goals.append((Tgrasp,pose,values)) elif self.irmodel.manip.FindIKSolution(Tgrasp,0) is None: numfailures += 1 self.armmanip = self.robot.GetManipulator('right_wam') self.robot.SetActiveManipulator('right_wam') print 'showing %d results'%N for ind,goal in enumerate(goals): raw_input('press ENTER to show goal %d'%ind) Tgrasp,base_pose, all_config = goal self.robot.SetTransform(base_pose) self.robot.SetDOFValues(all_config) # IPython.embed() idx = raw_input("Choose from goal index 0, 1, 2, 3, 4: ") goal_chosen = goals[int(idx)] Tgrasp, pose, all_config = goal_chosen matrix = openravepy.matrixFromPose(pose) aa = openravepy.axisAngleFromRotationMatrix(matrix) base_pose = [matrix[0,3], matrix[1,3], aa[2]] # x, y , theta return base_pose, all_config[11:18] # base_pose [x, y] grasp_config [7 dof values]
def PlotImages(self, o, a, desc): '''Produces plots of the robot's observation and selected action. - Input o: Image where 1st channel is the target sensed volume and the 2nd channel is the hand contents. - Input a: Index into the Q-function output which corresponds to the selected action. - Input desc: Descriptor corresponding to the current action in the base frame. - Returns None. ''' # setup fig = pyplot.figure() It = o[:, :, 0] Ih = zeros(It.shape) if self.IsGraspImage(o) else o[:, :, 1] Ir = copy(It); Ig = copy(It); Ib = copy(It) if self.IsOrientationLevel(): # determine rotation angle R = desc.T[0:3, 0:3] axisAngle = openravepy.axisAngleFromRotationMatrix(R) angle = norm(axisAngle) axis = axisAngle / angle if angle > 0 else array([0.0, 0.0, 1.0]) angle *= sign(sum(axis)) # draw axis indicator c = self.imP / 2 majorRadius = self.imP / 8 minorRadius = majorRadius if self.IsGraspImage else majorRadius / 2 xx, yy = ellipse_perimeter(c, c, minorRadius, majorRadius, orientation=0) Ir[xx, yy] = 1.0 # draw angle indicator length = self.imP / 5 x = -int(length * sin(angle)) y = int(length * cos(angle)) xx, yy = line(c, c, c + x, c + y) Ir[xx, yy] = 1.0 xx, yy = line(c, c, c, c + length) Ir[xx, yy] = 1.0 else: # draw the selection area halfWidth = (It.shape[0] * (self.selW / self.imW)) / 2.0 middle = It.shape[0] / 2.0 start = int(round(middle - halfWidth)) end = int(round(middle + halfWidth)) pixels = arange(start, end + 1) if start >= 0 and end < It.shape[0]: Ib[start, pixels] = 1.0 Ib[end, pixels] = 1.0 Ib[pixels, start] = 1.0 Ib[pixels, end] = 1.0 # draw robot's selection xh = self.actionsInHandFrame[a] xi = round(((xh[0:2] * self.imP) / self.imW) + ((self.imP - 1.0) / 2.0)).astype('int32') value = (xh[2] + (self.imD / 2.0)) / self.imD if xi[0] >= 0 and xi[1] < self.imP and xi[1] >= 0 and xi[1] < self.imP: Ir[xi[0], xi[1]] = value Ig[xi[0], xi[1]] = 0 Ib[xi[0], xi[1]] = 0 # show image Irgb = stack((Ir, Ig, Ib), 2) pyplot.subplot(1, 2, 1) pyplot.imshow(Irgb, vmin=0.00, vmax=1.00, interpolation="none") pyplot.subplot(1, 2, 2) pyplot.imshow(Ih, vmin=0.00, vmax=1.00, interpolation="none", cmap="gray") fig.suptitle("(Left.) Sensed volume. (Right.) Hand contents.") for i in xrange(2): fig.axes[i].set_xticks([]) fig.axes[i].set_yticks([]) pyplot.show(block=True)
T_w_k = berkeley_pr2.get_kinect_transform(robot) #o = T_w_k[:3,3] #x = T_w_k[:3,0] #y = T_w_k[:3,1] #z = T_w_k[:3,2] #handles = [] #handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1))) #handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1))) #handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1))) #viewer.Idle() T_h_k_init = berkeley_pr2.T_h_k xyz_init = T_h_k_init[:3, 3] rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3, :3]) f_init = 525 #from rapprentice.math_utils import linspace2d #sim_depths = calculate_sim_depths(xyz_init, rod_init, f_init) #cv2.imshow('hi',np.clip(sim_depths[0]/3,0,1)) #cv2.waitKey() #plot_real_and_sim(args2vec(xyz_init, rod_init, f_init)) #calculate_depth_error(xyz_init, rod_init, f_init) soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init, f_init), callback=plot_real_and_sim) (best_xyz, best_rod, best_f) = vec2args(soln) print "xyz, rod:", best_xyz, best_rod print "T_h_k:", calc_Thk(best_xyz, best_rod)
def get_object_list(self, req): #rospy.loginfo('Getting object list') resp = GetObjectListResponse() resp.in_goal = False resp.num_objects = 0 resp.objects = [] bodyNum = 0 for body in self.env_.GetBodies(): body_name = body.GetName().lower() resp.num_objects += 1 obj = ReconfigurationObject() s_pose = numpy.array([[0., -1., 0., 0.], [0., 0., 1., 0.], [-1., 0., 0., 0.], [0., 0., 0., 1.]]) s_inv = numpy.linalg.inv(s_pose) transform = body.GetTransform() q = transform[:3, :3] aa = openravepy.axisAngleFromRotationMatrix(q) state = self.module.GetState() obj_bb = body.ComputeAABB() if body_name == 'target': obj.type = 'target' obj.x = 1.0479 - state['movables'][bodyNum][0] obj.y = state['movables'][bodyNum][1] if self.initialize == True: self.target_off = aa[2] self.target_extents = [ obj_bb.extents()[0], obj_bb.extents()[1], obj_bb.extents()[2] ] obj.rot = state['movables'][bodyNum][2] + self.target_off obj.xextent = self.target_extents[0] obj.yextent = self.target_extents[1] obj.zextent = self.target_extents[2] in_goal = self.module.CheckGoal(state) if in_goal: resp.in_goal = True bodyNum = bodyNum + 1 elif body_name == 'fuze_bottle': obj.type = 'fuze_bottle' obj.x = state['movables'][bodyNum][0] obj.y = state['movables'][bodyNum][1] if self.initialize == True: self.bottle_radius = obj_bb.extents()[0] obj.xextent = self.bottle_radius bodyNum = bodyNum + 1 elif body_name in [ 'plastic_glass', 'plastic_glass2', 'plastic_glass3' ]: obj.type = 'plastic_glass' obj.x = state['movables'][bodyNum][0] obj.y = state['movables'][bodyNum][1] if self.initialize == True: self.glass_radius = obj_bb.extents()[0] obj.xextent = self.glass_radius bodyNum = bodyNum + 1 elif body_name in ['pop_tarts', 'pop_tarts2']: obj.type = 'pop_tarts' obj.x = 1.0479 - state['movables'][bodyNum][0] obj.y = state['movables'][bodyNum][1] if self.initialize == True: self.pop_tarts_off = aa[2] self.pop_tarts_extents = [ obj_bb.extents()[0], obj_bb.extents()[1], obj_bb.extents()[2] ] obj.rot = state['movables'][bodyNum][2] + self.pop_tarts_off obj.xextent = self.pop_tarts_extents[0] obj.yextent = self.pop_tarts_extents[1] obj.zextent = self.pop_tarts_extents[2] bodyNum = bodyNum + 1 elif body_name == 'bh280': obj.type = 'bh280' obj.x = state['manip'][0] obj.y = state['manip'][1] n_pose = body.GetTransform() now_in_start = numpy.dot(n_pose[:3, :3], s_inv[:3, :3]) obj.rot = numpy.arctan2(now_in_start[1, 0], now_in_start[0, 0]) if self.initialize == True: self.hand_off = aa[2] self.hand_extents = [ obj_bb.extents()[0], obj_bb.extents()[1], obj_bb.extents()[2] ] obj.xextent = self.hand_extents[0] obj.yextent = self.hand_extents[1] obj.zextent = self.hand_extents[2] resp.objects.append(obj) if self.initialize: self.startTime = datetime.now() self.initialize = False return resp
def PlotImagesContrastAdjusted(self, o, a, desc): '''Same as PlotImages but with contrast adjusted for visualization.''' # setup fig = pyplot.figure() It = o[:, :, 0] / max(o[:, :, 0]) Ih = zeros(It.shape) if self.IsGraspImage(o) else o[:, :, 1] / max(o[:, :, 1]) Ir = copy(It); Ig = copy(It); Ib = copy(It) if self.IsOrientationLevel(): # determine rotation angle R = desc.T[0:3, 0:3] axisAngle = openravepy.axisAngleFromRotationMatrix(R) angle = norm(axisAngle) axis = axisAngle / angle if angle > 0 else array([0.0, 0.0, 1.0]) angle *= sign(sum(axis)) # draw axis indicator c = self.imP / 2 majorRadius = self.imP / 8 minorRadius = majorRadius if self.IsGraspImage else majorRadius / 2 xx, yy = ellipse_perimeter(c, c, minorRadius, majorRadius, orientation=0) Ir[xx, yy] = 1.0 Ig[xx, yy] = 0.0 Ib[xx, yy] = 0.0 # draw angle indicator length = self.imP / 5 x = -int(length * sin(angle)) y = int(length * cos(angle)) xx, yy = line(c, c, c + x, c + y) Ir[xx, yy] = 1.0 Ig[xx, yy] = 0.0 Ib[xx, yy] = 0.0 xx, yy = line(c, c, c, c + length) Ir[xx, yy] = 1.0 Ig[xx, yy] = 0.0 Ib[xx, yy] = 0.0 else: # draw the selection area halfWidth = (It.shape[0] * (self.selW / self.imW)) / 2.0 middle = It.shape[0] / 2.0 start = int(round(middle - halfWidth)) end = int(round(middle + halfWidth)) pixels = arange(start, end + 1) if start >= 0 and end < It.shape[0]: Ir[start, pixels] = 0.0 Ir[end, pixels] = 0.0 Ir[pixels, start] = 0.0 Ir[pixels, end] = 0.0 Ig[start, pixels] = 0.0 Ig[end, pixels] = 0.0 Ig[pixels, start] = 0.0 Ig[pixels, end] = 0.0 Ib[start, pixels] = 1.0 Ib[end, pixels] = 1.0 Ib[pixels, start] = 1.0 Ib[pixels, end] = 1.0 # draw robot's selection xh = self.actionsInHandFrame[a] xi = round(((xh[0:2] * self.imP) / self.imW) + ((self.imP - 1.0) / 2.0)).astype('int32') cross = [(xi[0], xi[1]), (xi[0] + 1, xi[1]), (xi[0] - 1, xi[1]), (xi[0] + 2, xi[1]), (xi[0] - 2, xi[1]), (xi[0], xi[1]+ 1), (xi[0], xi[1] - 1), (xi[0], xi[1] + 2), (xi[0], xi[1] - 2)] for p in cross: if p[0] >= 0 and p[1] < self.imP and p[1] >= 0 and p[1] < self.imP: Ir[p[0], p[1]] = 1 Ig[p[0], p[1]] = 0 Ib[p[0], p[1]] = 0 # show image Irgb = stack((Ir, Ig, Ib), 2) pyplot.subplot(1, 2, 1) pyplot.imshow(Irgb, vmin=0.00, vmax=1.00, interpolation="none") pyplot.subplot(1, 2, 2) pyplot.imshow(Ih, vmin=0.00, vmax=1.00, interpolation="none", cmap="gray") fig.suptitle("(Left.) Sensed volume. (Right.) Hand contents.") for i in xrange(2): fig.axes[i].set_xticks([]) fig.axes[i].set_yticks([]) pyplot.show(block=True)
def GetCurrentConfiguration(self): t = self.robot.GetTransform() aa = openravepy.axisAngleFromRotationMatrix(t) pose = [t[0, 3], t[1, 3], aa[2]] return numpy.array(pose)
def axis_angle_from_rot(rot): return axisAngleFromRotationMatrix(rot)