def create_whole_body_trajectory(robot, arm_traj, base_traj): env = arm_traj.GetEnv() whole_timed_traj = RetimeWholeBodyTrajectory(robot, arm_traj, base_traj) save_trajectory(whole_timed_traj,'/home/abhi/Desktop/traj2/whole_body_timed_traj.xml') #save_trajectory(arm_timed_traj,'/home/abhi/Desktop/traj2/arm_timed_traj.xml') #save_trajectory(base_timed_traj,'/home/abhi/Desktop/traj2/base_timed_traj.xml') return whole_timed_traj
def executePath(robot, path, resolution, handles): print 'path: ' + str(len(path)) dis_poses = discretizePath(path, resolution) print 'dis_poses: ' + str(len(dis_poses)) poses = [] base_poses = [] all_poses = [] all_poses.append(dis_poses) base_goal = [] arm_goal = [] for i in range(len(dis_poses) - 1): pose, base_pose, finalgoal = executeVelPath(robot, dis_poses[i + 1], handles) arm_goal.append(finalgoal[:8]) base_goal.append(finalgoal[-3:]) poses.append(pose) base_poses.append(base_pose) all_poses.append(poses) all_poses.append(base_poses) print 'size of base points: ' + str(len(base_goal)) print 'size of arm points: ' + str(len(arm_goal)) from prpy.rave import save_trajectory #Creating base Traj doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis cspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot) base_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), 'GenericTrajectory') base_traj.Init(cspec) for i in range(len(base_goal)): base_traj.Insert(i, base_goal[i]) save_trajectory(base_traj, '/home/abhi/Desktop/traj2/base_untimed_traj.xml') print 'base_traj saved' #Creating Arm Traj jointnames = [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] robot.SetActiveDOFs( [robot.GetJoint(name).GetDOFIndex() for name in jointnames], ) acspec = robot.GetActiveConfigurationSpecification('linear') arm_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '') arm_traj.Init(acspec) for i in range(len(arm_goal)): arm_traj.Insert(i, arm_goal[i]) save_trajectory(arm_traj, '/home/abhi/Desktop/traj2/arm_untimed_traj.xml') print 'arm_traj saved' return all_poses
def executePath(robot, path, resolution, handles): manip = robot.SetActiveManipulator('arm_torso') print 'path: ' + str(len(path)) dis_poses = discretizePath(path, resolution) print 'dis_poses: ' + str(len(dis_poses)) poses = [] base_poses = [] all_poses = [] all_poses.append(dis_poses) jointnames = [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] robot.SetActiveDOFs( [robot.GetJoint(name).GetDOFIndex() for name in jointnames], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) cspec = robot.GetActiveConfigurationSpecification('linear') traj = openravepy.RaveCreateTrajectory(env, '') cspec.AddGroup('joint_velocities', dof=8, interpolation='quadratic') cspec.AddGroup('affine_velocities', dof=4, interpolation='next') cspec.AddDeltaTimeGroup() traj.Init(cspec) #Creating the first point of the trajectory (the current joint values of the robot) arm_curr = robot.GetDOFValues(manip.GetArmIndices()) base_curr = np.zeros(3) base_vel = np.zeros(4) arm_vel_cuur = np.zeros(8) dt = 0. value = [] value.extend(arm_curr) value.extend(base_curr) value.extend(arm_vel_cuur) value.extend(base_vel) value.extend([dt]) traj.Insert(0, value) curr_time = round(time.time() * 1000) for i in range(len(dis_poses) - 1): pose, base_pose, arm_vel, finalgoal = executeVelPath(robot, dis_poses[i + 1], handles, unitTime=1.0) # now creating other waypoints of the trajectory value = [] value.extend(finalgoal[:8]) value.extend(finalgoal[-3:]) value.extend(arm_vel) value.extend(np.zeros(3)) time_now = round(time.time() * 1000) dt = time_now - curr_time value.extend([dt / 5000.]) value.extend([dt / 5000.]) traj.Insert(i + 1, value) poses.append(pose) base_poses.append(base_pose) save_trajectory( traj, '/home/abhi/Desktop/traj2/whole_body_zigzag_timed_traj.xml') all_poses.append(poses) all_poses.append(base_poses) #print 'size of base points: '+str(len(base_goal)) #print 'size of arm points: '+str(len(arm_goal)) # #Creating base Traj # doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis # cspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot) # base_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), 'GenericTrajectory') # base_traj.Init(cspec) # for i in range(len(base_goal)): # base_traj.Insert(i, base_goal[i]) # save_trajectory(base_traj,'/home/abhi/Desktop/traj2/base_untimed_traj.xml') # print 'base_traj saved' # #Creating Arm Traj # jointnames=['torso_lift_joint','shoulder_pan_joint','shoulder_lift_joint','upperarm_roll_joint','elbow_flex_joint','forearm_roll_joint','wrist_flex_joint','wrist_roll_joint'] # robot.SetActiveDOFs([robot.GetJoint(name).GetDOFIndex() for name in jointnames],) # acspec = robot.GetActiveConfigurationSpecification('linear') # arm_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '') # arm_traj.Init(acspec) # for i in range(len(arm_goal)): # arm_traj.Insert(i, arm_goal[i]) # save_trajectory(arm_traj,'/home/abhi/Desktop/traj2/arm_untimed_traj.xml') # print 'arm_traj saved' return all_poses
maxiter=5000, steplength=10, maxtries=2) waitrobot(robot) value = [] value.extend(rotate_neg) value.extend(vel) time_now = round(time.time() * 1000) dt = time_now - curr_time value.extend([dt / 1000.]) value.extend([dt / 1000.]) traj.Insert(7, value) # Coming back to center basemanip.MoveActiveJoints(goal=curr, maxiter=5000, steplength=10, maxtries=2) waitrobot(robot) value = [] value.extend(curr) value.extend(vel) time_now = round(time.time() * 1000) dt = time_now - curr_time value.extend([dt / 1000.]) value.extend([dt / 1000.]) traj.Insert(8, value) save_trajectory(traj, '/home/abhi/Desktop/traj2/whole_body_ee_fixed_traj.xml')
def executePath(self, path, resolution, handles, traj_name='whole_body_traj.xml'): dis_poses = self.discretizePath(path, resolution) poses = [] base_poses = [] all_poses = [] all_poses.append(dis_poses) jointnames = [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] self.robot.SetActiveDOFs( [self.robot.GetJoint(name).GetDOFIndex() for name in jointnames], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis) cspec = self.robot.GetActiveConfigurationSpecification('linear') traj = RaveCreateTrajectory(self.robot.GetEnv(), '') cspec.AddGroup('joint_velocities', dof=8, interpolation='quadratic') cspec.AddGroup('affine_velocities', dof=4, interpolation='next') cspec.AddDeltaTimeGroup() traj.Init(cspec) #Creating the first point of the trajectory (the current joint values of the robot) arm_curr = self.robot.GetDOFValues(self.manip.GetArmIndices()) base_curr = np.zeros(3) base_vel = np.zeros(4) arm_vel_cuur = np.zeros(8) dt = 0. value = [] value.extend(arm_curr) value.extend(base_curr) value.extend(arm_vel_cuur) value.extend(base_vel) value.extend([dt]) traj.Insert(0, value) curr_time = round(time.time() * 1000) for i in range(len(dis_poses) - 1): pose, base_pose, arm_vel, finalgoal = self.executeVelPath( dis_poses[i + 1], handles, unitTime=1.0) # now creating other waypoints of the trajectory value = [] value.extend(finalgoal[:8]) value.extend(finalgoal[-3:]) value.extend(arm_vel) value.extend(np.zeros(3)) time_now = round(time.time() * 1000) dt = time_now - curr_time value.extend([dt / 5000.]) value.extend([dt / 5000.]) traj.Insert(i + 1, value) poses.append(pose) base_poses.append(base_pose) rospack = rospkg.RosPack() path = rospack.get_path('fetchwbp') filename = path + '/trajectories/' + traj_name save_trajectory(traj, filename) all_poses.append(poses) all_poses.append(base_poses) return all_poses