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