def SetEnd(self, robotid, currentik, initik, T0_TSY):
robotid.SetActiveDOFValues(currentik)
T_rh = None
T_lf = None
# Defines a box in which to have the end-effector manipulate
if self.use_manipbox :
hand_box = matrix( self.manipbox_dim + [-1000,1000,-1000,1000,-1000,1000] )
T_rh = T0_TSY
T_lh = T0_TSY
else :
hand_box = matrix([-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000])
T_rh = self.robotManips[1].GetEndEffectorTransform()
T_lh = self.robotManips[0].GetEndEffectorTransform()
# Define Task Space Region strings
# Left Hand
TSRStringLH1 = SerializeTSR(0,'NULL',T_lh,eye(4),hand_box)
TSRStringLH0 = SerializeTSR(0,'NULL',self.robotManips[0].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Right Hand
TSRStringRH1 = SerializeTSR(1,'NULL',T_rh,eye(4),hand_box)
TSRStringRH0 = SerializeTSR(1,'NULL',self.robotManips[1].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Left Foot
TSRStringLF1 = SerializeTSR(2,'NULL',self.robotManips[2].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Right Foot
TSRStringRF1 = SerializeTSR(3,'NULL',self.robotManips[3].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
# Head
TSRStringH = SerializeTSR(4,'NULL',self.robotManips[4].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
if(type(initik) == type("")):
robotid.SetActiveDOFValues(str2num(initik))
else:
robotid.SetActiveDOFValues(initik)
robotid.GetController().Reset(0)
# Left Foot
TSRStringLF2 = SerializeTSR(2,'NULL',self.robotManips[2].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,-100,100,0,0,0,0,0,0]))
# Right Foot
TSRStringRF2 = SerializeTSR(3,'NULL',self.robotManips[3].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,-100,100,0,0,0,0,0,0]))
# We have the strings. Let's chain them together.
self.TSRChainStringFeetandHead_current2init_bh = SerializeTSRChain(0,0,1,1,TSRStringLH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
self.TSRChainStringFeetandHead_current2init_lh = SerializeTSRChain(0,0,1,1,TSRStringRH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
self.TSRChainStringFeetandHead_current2init_rh = SerializeTSRChain(0,0,1,1,TSRStringLH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
self.TSRChainStringFeetandHead_init2home = SerializeTSRChain(0,0,1,1,TSRStringLF2,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF2,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
def traj2ach(env,robot,traj,fname,robotJointValsOffset,robotJointVelsOffset,deltatimeOffset):
myAchTraj=openravepy.RaveCreateTrajectory(robot.GetEnv(),'')
config=deepcopy(traj.GetConfigurationSpecification())
robotJointValsDOF = traj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).dof
robotJointVelsDOF = traj.GetConfigurationSpecification().GetGroupFromName("joint_velocities "+robot.GetName()).dof
#config.AddDeltaTimeGroup()
myAchTraj.Init(config)
trajLength = traj.GetNumWaypoints()
numJoints = len(robot.GetJoints())
freq = 1.0/25.0
for i in range(trajLength):
wp = traj.GetWaypoint(i)
awp = deepcopy(wp)
# print "joint values"
q = wp[robotJointValsOffset:(robotJointValsOffset+robotJointValsDOF)]
#print "Rave2RealHubo says"
#print robotJointValsOffset
#print (robotJointValsOffset+numJoints)
# print q
# print "joint velocities"
qdot = wp[robotJointVelsOffset:(robotJointVelsOffset+robotJointVelsDOF)]
# print qdot
# print "deltatime"
dt = wp[deltatimeOffset]
# print dt
if(dt != 0.0):
awp[deltatimeOffset] = 0.5
myAchTraj.Insert(i,awp)
# pose['RHY'] = q[1]
# pose['RHR'] = q[2]
# pose['RHP'] = q[3]
# pose['RKN'] = q[4]
# pose['RAP'] = q[5]
# pose['RAR'] = q[6]
# pose['LHY'] = q[26]
# pose['LHR'] = q[27]
# pose['LHP'] = q[28]
# pose['LKN'] = q[29]
# pose['LAP'] = q[30]
# pose['LAR'] = q[31]
# pose['RSP'] = q[7]
# pose['RSR'] = q[8]
# pose['RSY'] = q[9]
# pose['REB'] = q[10]
# pose['RWY'] = q[11]
# pose['RWR'] = q[13]
# pose['RWP'] = q[12]
# pose['LSP'] = q[19]
# pose['LSR'] = q[20]
# pose['LSY'] = q[21]
# pose['LEB'] = q[22]
# pose['LWY'] = q[23]
# pose['LWR'] = q[25]
# pose['LWP'] = q[24]
# pose['NKY'] = q[14]
# pose['NK1'] = q[15]
# pose['NK2'] = q[16]
# pose['WST'] = q[0]
# pose['RF1'] = q[18]
# pose['RF2'] = q[17]
# pose['RF3'] = 0
# pose['RF4'] = 0
# pose['RF5'] = 0
# pose['LF1'] = q[32]
# pose['LF2'] = 0
# pose['LF3'] = 0
# pose['LF4'] = 0
# pose['LF5'] = 0
# openravepy.planningutils.RetimeTrajectory(myAchTraj,hastimestamps=True) # inputs: trajectory, hastimestamps.
openravepy.planningutils.RetimeActiveDOFTrajectory(myAchTraj,robot,hastimestamps=False,maxvelmult=1,maxaccelmult=1,plannername='ParabolicTrajectoryRetimer')
achTrajLength = myAchTraj.GetNumWaypoints()
retimedRobotJointValsOffset = myAchTraj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).offset
retimedRobotJointValsDOF = myAchTraj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).dof
activedofs = str2num(myAchTraj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).name[len("joint_values "+robot.GetName()):]).astype(int)
fRaveRetimed = open(fname+'_retimed.txt','w')
fRaveRetimed.write(myAchTraj.serialize(0))
fRaveRetimed.close()
f = open(fname+'.traj','w')
for i in range(achTrajLength):
atwp = myAchTraj.GetWaypoint(i)
allq = zeros(numJoints)
aq = atwp[retimedRobotJointValsOffset:(retimedRobotJointValsOffset+retimedRobotJointValsDOF)]
for aIdx, a in enumerate(activedofs):
allq[a] = aq[aIdx]
# No Fingers
myAchQ = [allq[1], allq[2], allq[3], allq[4], allq[5], allq[6], allq[26], allq[27], allq[28], allq[29], allq[30], allq[31], allq[7], allq[8], allq[9], allq[10], allq[11], allq[13], allq[12], allq[19], allq[20], allq[21], allq[22], allq[23], allq[25], allq[24], allq[14], allq[15], allq[16], allq[0], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# print myAchQ
f.write(' '.join([str(x) for x in myAchQ])+'\n')
f.close()
def traj2ach(env, robot, traj, fname, robotJointValsOffset,
robotJointVelsOffset, deltatimeOffset):
myAchTraj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '')
config = deepcopy(traj.GetConfigurationSpecification())
robotJointValsDOF = traj.GetConfigurationSpecification().GetGroupFromName(
"joint_values " + robot.GetName()).dof
robotJointVelsDOF = traj.GetConfigurationSpecification().GetGroupFromName(
"joint_velocities " + robot.GetName()).dof
#config.AddDeltaTimeGroup()
myAchTraj.Init(config)
trajLength = traj.GetNumWaypoints()
numJoints = len(robot.GetJoints())
freq = 1.0 / 25.0
for i in range(trajLength):
wp = traj.GetWaypoint(i)
awp = deepcopy(wp)
# print "joint values"
q = wp[robotJointValsOffset:(robotJointValsOffset + robotJointValsDOF)]
#print "Rave2RealHubo says"
#print robotJointValsOffset
#print (robotJointValsOffset+numJoints)
# print q
# print "joint velocities"
qdot = wp[robotJointVelsOffset:(robotJointVelsOffset +
robotJointVelsDOF)]
# print qdot
# print "deltatime"
dt = wp[deltatimeOffset]
# print dt
if (dt != 0.0):
awp[deltatimeOffset] = 0.5
myAchTraj.Insert(i, awp)
# pose['RHY'] = q[36]
# pose['RHR'] = q[37]
# pose['RHP'] = q[38]
# pose['RKN'] = q[39]
# pose['RAP'] = q[40]
# pose['RAR'] = q[41]
# pose['LHY'] = q[11]
# pose['LHR'] = q[12]
# pose['LHP'] = q[13]
# pose['LKN'] = q[14]
# pose['LAP'] = q[15]
# pose['LAR'] = q[16]
# pose['RSP'] = q[26]
# pose['RSR'] = q[27]
# pose['RSY'] = q[28]
# pose['REB'] = q[29]
# pose['RWY'] = q[30]
# pose['RWR'] = q[32]
# pose['RWP'] = q[31]
# pose['LSP'] = q[0]
# pose['LSR'] = q[1]
# pose['LSY'] = q[2]
# pose['LEB'] = q[3]
# pose['LWY'] = q[4]
# pose['LWR'] = q[6]
# pose['LWP'] = q[5]
# pose['NKY'] = q[17]
# pose['NK1'] = q[18]
# pose['NK2'] = q[19]
# pose['WST'] = q[10]
# pose['RF1'] = q[33]
# pose['RF2'] = q[42]
# pose['RF3'] = q[45]
# pose['RF4'] = q[48]
# pose['RF5'] = 0
# pose['LF1'] = q[7]
# pose['LF2'] = q[20]
# pose['LF3'] = q[23]
# pose['LF4'] = 0
# pose['LF5'] = 0
# openravepy.planningutils.RetimeTrajectory(myAchTraj,hastimestamps=True) # inputs: trajectory, hastimestamps.
openravepy.planningutils.RetimeActiveDOFTrajectory(
myAchTraj,
robot,
hastimestamps=False,
maxvelmult=1,
maxaccelmult=1,
plannername='ParabolicTrajectoryRetimer')
achTrajLength = myAchTraj.GetNumWaypoints()
retimedRobotJointValsOffset = myAchTraj.GetConfigurationSpecification(
).GetGroupFromName("joint_values " + robot.GetName()).offset
retimedRobotJointValsDOF = myAchTraj.GetConfigurationSpecification(
).GetGroupFromName("joint_values " + robot.GetName()).dof
activedofs = str2num(
myAchTraj.GetConfigurationSpecification().GetGroupFromName(
"joint_values " +
robot.GetName()).name[len("joint_values " +
robot.GetName()):]).astype(int)
fRaveRetimed = open(fname + '_retimed.txt', 'w')
fRaveRetimed.write(myAchTraj.serialize(0))
fRaveRetimed.close()
f = open(fname + '.traj', 'w')
for i in range(achTrajLength):
atwp = myAchTraj.GetWaypoint(i)
allq = zeros(numJoints)
aq = atwp[retimedRobotJointValsOffset:(retimedRobotJointValsOffset +
retimedRobotJointValsDOF)]
for aIdx, a in enumerate(activedofs):
# With Fingers
# myAchQ = [q[36], q[37], q[38], q[39], q[40], q[41], q[11], q[12], q[13], q[14], q[15], q[16], q[26], q[27], q[28], q[29], q[30], q[32], q[31], q[0], q[1], q[2], q[3], q[4], q[6], q[5], q[17], q[18], q[19], q[10], q[33], q[42], q[45], q[48], 0, q[7], q[20], q[23], 0, 0]
allq[a] = aq[aIdx]
# No Fingers
myAchQ = [
allq[36], allq[37], allq[38], allq[39], allq[40], allq[41],
allq[11], allq[12], allq[13], allq[14], allq[15], allq[16],
allq[26], allq[27], allq[28], allq[29], allq[30], allq[32],
allq[31], allq[0], allq[1], allq[2], allq[3], allq[4], allq[6],
allq[5], allq[17], allq[18], allq[19], allq[10], 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
]
# print myAchQ
f.write(' '.join([str(x) for x in myAchQ]) + '\n')
f.close()