def is_valid(self,config,pname):
if config == '':
print "Hey: No IK Solution found! Move the robot!"
return 0
else:
print str2num(config)
print type(str2num(config))
self.robot.SetActiveDOFValues(str2num(config))
if(self.env.CheckCollision(self.robot,self.crankid)):
print "Point is in collision."
return 0
else:
print "Point "+pname+" validated"
print "------"
return 1
def go_to_start_config_and_pose(myRobot, myRmaps, sol, T0_starts):
for myManipulatorIndex in range(2):
# Find where to move the base
startSphereIndex = sol[myManipulatorIndex][0].sIdx
startTransformIndex = sol[myManipulatorIndex][0].tIdx
myRmaps[myManipulatorIndex].go_to(startSphereIndex,startTransformIndex)
Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[startTransformIndex]
T0_newManipPose = dot(T0_starts[myManipulatorIndex],linalg.inv(Tbase_start))
myRobot.SetTransform(array(T0_newManipPose))
startConfigStr = sol[2]
myRobot.SetActiveDOFValues(str2num(startConfigStr))
time.sleep(0.05)
def generateChain(self, offsetHandleToHand, handlePose, Bw0, Bw1,
furnitureName, jointIndex, cbirrtProb):
jointtm = str2num(
cbirrtProb.SendCommand('GetJointTransform name ' + furnitureName +
' jointind %d' % jointIndex))
T0_w0 = MakeTransform(handlePose[:3][:, 0:3], mat(jointtm[9:12]).T)
Tw1_e = offsetHandleToHand
Tw0_e = linalg.inv(T0_w0) * handlePose
TSRstring1 = SerializeTSR(0, 'NULL', T0_w0, Tw0_e, Bw0)
TSRstring2 = SerializeTSR(0, 'NULL', mat(eye(4)), Tw1_e, Bw1)
TSRChainString = SerializeTSRChain(0, 0, 1, 2,
TSRstring1 + ' ' + TSRstring2,
furnitureName, mat(jointIndex))
return TSRChainString
def go_to_start_config_and_pose(myRobot, myRmaps, sol, T0_starts):
for myManipulatorIndex in range(2):
# Find where to move the base
startSphereIndex = sol[myManipulatorIndex][0].sIdx
startTransformIndex = sol[myManipulatorIndex][0].tIdx
myRmaps[myManipulatorIndex].go_to(startSphereIndex,
startTransformIndex)
Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[
startTransformIndex]
T0_newManipPose = dot(T0_starts[myManipulatorIndex],
linalg.inv(Tbase_start))
myRobot.SetTransform(array(T0_newManipPose))
startConfigStr = sol[2]
myRobot.SetActiveDOFValues(str2num(startConfigStr))
time.sleep(0.05)
def is_valid(self, config, pname):
if config == "":
# print"Hey: No IK Solution found! Move the robot!"
return 0
else:
# printstr2num(config)
# printtype(str2num(config))
self.robot.SetActiveDOFValues(str2num(config))
# sys.stdin.readline()
if self.env.CheckCollision(self.robot, self.crankid):
print "Point " + pname + " is in collision."
return 0
else:
print "Point " + pname + " is valid"
# print"------"
return 1
def test_points(self):
self.robot.SetActiveDOFValues(str2num(self.approachik_1))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_1),self.robot.GetActiveDOFIndices())
print "went to approach-1"
sys.stdin.readline()
self.robot.SetActiveDOFValues(str2num(self.approachik_2))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_2),self.robot.GetActiveDOFIndices())
print "went to approach-2"
sys.stdin.readline()
self.robot.SetActiveDOFValues(str2num(self.startik))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices())
print "went to startik"
sys.stdin.readline()
self.robot.SetActiveDOFValues(str2num(goalik)) # Note: self.goalik is T0_LH2 and T0_RH2
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.goalik),self.robot.GetActiveDOFIndices())
print "went to goalik"
sys.stdin.readline()
self.robot.SetActiveDOFValues(str2num(startik))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices())
################################################
## YOU'RE AT STARTIK - GO BACK TO APPROACH 2 ##
################################################
goaljoints = str2num(self.approachik_2);
print "Press enter to go to approach point 2"
sys.stdin.readline()
self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices())
##################################################
## YOU'RE AT APPROACH 2 - GO BACK TO APPROACH 1 ##
##################################################
goaljoints = str2num(self.approachik_1);
print "Press enter to go to approach point 1"
sys.stdin.readline()
self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices())
######################################################
## YOU'RE AT APPROACH 1 - GO BACK TO INITCONFIG ##
######################################################
print "Press enter to go to initconfig"
sys.stdin.readline()
self.SetRobotDOFValuesInSyncWithROS(self.robot,self.initconfig,self.robot.GetActiveDOFIndices())
return 1
class DrcHuboWheelTurning(BaseWheelTurning):
def __init__(
self,
HuboModelPath='../../openHubo/drchubo/robots/drchubo.robot.xml',
WheelModelPath='../../../drc_common/models/driving_wheel.robot.xml'
):
BaseWheelTurning.__init__(self, HuboModelPath, WheelModelPath)
# Set those variables to show or hide the interface
# Do it using the member functions
self.StopAtKeyStrokes = False
self.ShowUserInterface = False
self.ViewerStarted = False
# Right Hand Joints
# Open - Closed Values
self.rhanddofs = range(26, 29)
self.rhandclosevals = [0.95, 0.95, -0.95]
self.rhandopenvals = [0, 0, 0]
# Left Hand Joints
self.lhanddofs = range(8, 11)
self.lhandclosevals = [0.95, 0.95, -0.95]
self.lhandopenvals = [0, 0, 0]
def Run(self):
self.RemoveFiles()
# This is a list of handles of the objects that are
# drawn on the screen in OpenRAVE Qt-Viewer.
# Keep appending to the end, and pop() if you want to delete.
handles = []
normalsmoothingitrs = 150
fastsmoothingitrs = 20
self.StartViewerAndSetWheelPos(handles)
# Wheel Joint Index
crankjointind = 0
# Set the wheel joints back to 0 for replanning
self.crankid.SetDOFValues([0], [crankjointind])
self.crankid.GetController().Reset(0)
shiftUp = 0.95
# Move the wheel infront of the robot
self.crankid.SetTransform(
array(
MakeTransform(
dot(rodrigues([0, 0, pi / 2]), rodrigues([pi / 2, 0, 0])),
transpose(matrix([0.5, 0.0, shiftUp])))))
probs_cbirrt = RaveCreateModule(self.env, 'CBiRRT')
probs_crankmover = RaveCreateModule(self.env, 'CBiRRT')
manips = self.robotid.GetManipulators()
crankmanip = self.crankid.GetManipulators()
try:
self.env.AddModule(probs_cbirrt, self.robotid.GetName())
self.env.AddModule(probs_crankmover, self.crankid.GetName())
except openrave_exception, e:
print e
print "Getting Loaded Problems"
probs = self.env.GetLoadedProblems()
# Keep Active Joint Indices
activedofs = [0]
for m in manips:
activedofs.extend(m.GetArmIndices())
# Sort Active Joint Indices
activedofs.sort()
# elbows: Left Elbow Pitch: 4; Right Elbow Pitch: 22
self.robotid.SetDOFValues([-0.95, -0.95], [4, 22])
self.robotid.SetActiveDOFs(activedofs)
# Current configuration of the robot is its initial configuration
initconfig = self.robotid.GetActiveDOFValues()
# List of Robot Links
links = self.robotid.GetLinks()
# List of Wheel (Crank Links)
cranklinks = self.crankid.GetLinks()
# End Effector Transforms
Tee = []
for i in range(len(manips)):
# Returns End Effector Transform in World Coordinates
Tlink = manips[i].GetEndEffectorTransform()
Tee.append(Tlink)
# Wheel Joint Index
crankjointind = 0
# Get Transformation Matrix for the Wheel
# Note that crank's links are not rotated
# If you want use the wheel's end effector's transformation
# matrix (which is 23 degrees tilted) then see
# CTee matrix below.
#
# crank has two links:
# 0) pole - the blue cylinder in the model, and,
# 1) crank - the driving wheel itself.
jointtm = cranklinks[0].GetTransform()
# handles.append(misc.DrawAxes(env,matrix(jointtm),1))
# We can also get the transformation matrix
# with the following command as a string
jointtm_str = probs[0].SendCommand(
'GetJointTransform name crank jointind ' + str(crankjointind))
# And then we can convert the string to a 1x12 array
jointtm_str = jointtm_str.replace(" ", ",")
jointtm_num = eval('[' + jointtm_str + ']')
# In this script we will use jointtm.
# jointtm_str and jointtm_num are given as example.
# Crank Transform End Effector in World Coordinates
# This is the transformation matrix of the end effector
# named "dummy" in the xml file.
# Note that dummy is tilted 23 degress around its X-Axis
CTee = crankmanip[0].GetEndEffectorTransform()
tilt_angle_deg = acos(dot(linalg.inv(CTee), jointtm)[1, 1]) * 180 / pi
tilt_angle_rad = acos(dot(linalg.inv(CTee), jointtm)[1, 1])
# Center of Gravity Target
# cogtarg = [-0.05, 0.085, 0]
cogtarg = [0, 0, 0]
# polyscale: changes the scale of the support polygon
# polytrans: shifts the support polygon around
footlinknames = ' Body_RAR Body_LAR ' #polytrans -0.015 0 0.0 '
# What is this?
handrot = rodrigues([0, -pi / 2, 0])
# Translation Offset from the wheel center for the hands
transoffset = [0, 0.15, 0]
# Figure out where to put the left hand on the wheel
temp = dot(
CTee,
MakeTransform(rodrigues([0, -pi / 2, 0]),
transpose(matrix([0, 0, 0]))))
temp = dot(
temp,
MakeTransform(rodrigues([-pi, 0, 0]), transpose(matrix([0, 0,
0]))))
# Left Hand Pose in World Coordinates
T0_LH1 = dot(
temp,
MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([0, 0,
0.145]))))
# Uncomment if you want to see where T0_LH1 is
handles.append(misc.DrawAxes(self.env, matrix(T0_LH1), 1))
# Figure out where to put the right hand on the wheel
temp = dot(
CTee,
MakeTransform(rodrigues([0, -pi / 2, 0]),
transpose(matrix([0, 0, 0]))))
# Right Hand Pose in World Coordinates
T0_RH1 = dot(
temp,
MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([0, 0,
0.145]))))
# Uncomment if you want to see where T0_RH1 is
handles.append(misc.DrawAxes(self.env, matrix(T0_RH1), 1))
# Define Task Space Region strings
# Left Hand
TSRString1 = SerializeTSR(0, 'NULL', T0_LH1, eye(4),
matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Right Hand
TSRString2 = SerializeTSR(1, 'NULL', T0_RH1, eye(4),
matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Left Foot
TSRString3 = SerializeTSR(2, 'NULL', Tee[2], eye(4),
matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Right Foot
TSRString4 = SerializeTSR(3, 'NULL', Tee[3], eye(4),
matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Head
# Grasp transform in Head coordinates
Tw0_eH = eye(4)
# How much freedom do we want to give to the Head
# [x,x,y,y,z,z,R,R,P,P,Y,Y]
Bw0H = matrix(
[-0.05, 0.05, -0.1, 0.1, -100, 100, -pi, pi, -pi, pi, -pi, pi])
TSRString5 = SerializeTSR(4, 'NULL', Tee[4], Tw0_eH, Bw0H)
# We defined Task Space Regions. Now let's concatenate them.
TSRChainStringGrasping = SerializeTSRChain(
0, 1, 0, 1, TSRString1, 'NULL', []) + ' ' + SerializeTSRChain(
0, 1, 0, 1, TSRString2, 'NULL', []) + ' ' + SerializeTSRChain(
0, 1, 1, 1, TSRString3,
'NULL', []) + ' ' + SerializeTSRChain(
0, 1, 1, 1, TSRString4, 'NULL', []
) #+' '+SerializeTSRChain(0,1,0,1,TSRString5,'NULL',[])
if (self.StopAtKeyStrokes):
print "Press Enter to go to startik"
sys.stdin.readline()
startik = probs[0].SendCommand(
'DoGeneralIK exec nummanips 2 maniptm 0 ' + trans_to_str(T0_LH1) +
' maniptm 1 ' + trans_to_str(T0_RH1))
# Rotate the wheel's transform to a suitable pose
# for the Left Hand
# T0_w0L stands for:
# left hand's transform on wheel in world coordinates
T0_w0L = MakeTransform(rodrigues([0, tilt_angle_rad, 0]),
transpose(matrix(jointtm[0:3, 3])))
# This is what's happening:
#
# Tw0L_0 = linalg.inv(T0_w0L)
# Tw0L_LH1 = Tw0L_0*T0_LH1
#
# Left hand's transform in wheel's coordinates
Tw0L_LH1 = dot(linalg.inv(T0_w0L), T0_LH1)
# Transform of the left hand's end effector in wheel's coords.
# Required by CBiRRT
Tw0_eL = Tw0L_LH1
# How much freedom do we want to give to the left hand
Bw0L = matrix([0, 0, 0, 0, 0, 0, 0, pi, 0, 0, 0, 0])
# Right Hand's transforms:
T0_crankcrank = self.crankid.GetManipulators()[0].GetTransform()
T0_w0R = MakeTransform(rodrigues([tilt_angle_rad, 0, 0]),
transpose(matrix([0, 0, 0])))
# End effector transform in wheel coordinates
Tw0_eR = dot(linalg.inv(T0_crankcrank), T0_RH1)
# handles.append(misc.DrawAxes(env,matrix(Tw0_eR),1))
# How much freedom? (note: in frame of crank)
Bw0R = matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
# Head's transforms:
T0_w0H = Tee[4]
Tw0_eH = eye(4)
Bw0H = matrix(
[-0.05, 0.05, -0.1, 0.1, -100, 100, -pi, pi, -pi, pi, -pi, pi])
# Define Task Space Regions
# Left Hand
TSRString1 = SerializeTSR(0, 'NULL', T0_w0L, Tw0_eL, Bw0L)
# Right Hand
TSRString2 = SerializeTSR(1, 'crank crank', T0_w0R, Tw0_eR, Bw0R)
# Left Foot
TSRString3 = SerializeTSR(2, 'NULL', Tee[2], eye(4),
matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Right Foot
TSRString4 = SerializeTSR(3, 'NULL', Tee[3], eye(4),
matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
# Head
TSRString5 = SerializeTSR(4, 'NULL', T0_w0H, Tw0_eH, Bw0H)
TSRChainStringFeetandHead = SerializeTSRChain(
0, 0, 1, 1, TSRString3, 'NULL', []) + ' ' + SerializeTSRChain(
0, 0, 1, 1, TSRString4, 'NULL', []) + ' ' + SerializeTSRChain(
0, 0, 1, 1, TSRString5, 'NULL', [])
TSRChainString = SerializeTSRChain(
0, 0, 1, 1, TSRString1, 'crank', matrix(
[crankjointind])) + ' ' + SerializeTSRChain(
0, 0, 1, 1, TSRString2, 'NULL', matrix(
[])) + ' ' + TSRChainStringFeetandHead
# Calculate hand transforms after rotating the wheel (they will help us find the goalik):
# How much do we want to rotate the wheel?
crank_rot = pi / 6.5
# Which joint do we want the CBiRRT to mimic the TSR for?
TSRChainMimicDOF = 1
# Create the transform for the wheel that we would like to reach to
Tcrank_rot = MakeTransform(rodrigues([crank_rot, 0, 0]),
transpose(matrix([0, 0, 0])))
# What is this?
temp = MakeTransform(rodrigues([0, 0, crank_rot]),
transpose(matrix([0, 0, 0])))
# Rotate the left hand's transform on the wheel in world transform "crank_rot" radians around it's Z-Axis
T0_cranknew = dot(T0_w0L, Tcrank_rot)
# Where will the left hand go after turning the wheel?
# This is what's happening:
#
# Tcranknew_LH2 = dot(Tw0L_0,T0_LH1) --> Left hand in wheel's coordinate
# T0_LH2 = dot(T0_cranknew,Tcranknew_LH2) --> Left hand rotated around wheel's origin
T0_LH2 = dot(T0_cranknew, dot(linalg.inv(T0_w0L), T0_LH1))
# Uncomment to see T0_LH2
handles.append(misc.DrawAxes(self.env, matrix(T0_LH2), 1))
# Where will the right hand go after turning the wheel?
T0_RH2 = dot(T0_crankcrank,
dot(temp, dot(linalg.inv(T0_crankcrank), T0_RH1)))
# Uncomment to see T0_RH2
handles.append(misc.DrawAxes(self.env, matrix(T0_RH2), 1))
if (self.StopAtKeyStrokes):
print "Press Enter to find a goalIK"
sys.stdin.readline()
self.crankid.SetDOFValues([crank_rot], [crankjointind])
arg1 = str(cogtarg).strip("[]").replace(', ', ' ')
arg2 = trans_to_str(T0_LH2)
arg3 = trans_to_str(T0_RH2)
goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' +
footlinknames + ' movecog ' + arg1 +
' nummanips 2 maniptm 0 ' + arg2 +
' maniptm 1 ' + arg3)
self.robotid.SetActiveDOFValues(str2num(goalik))
self.crankid.SetDOFValues([crank_rot], [crankjointind])
self.crankid.SetDOFValues([0], [crankjointind])
self.robotid.SetActiveDOFValues(initconfig)
self.robotid.SetDOFValues(self.rhandopenvals, self.rhanddofs)
self.robotid.SetDOFValues(self.lhandopenvals, self.lhanddofs)
if (self.StopAtKeyStrokes):
print "Press Enter to plan initconfig --> startik"
sys.stdin.readline()
# Get a trajectory from initial configuration to grasp configuration
with self.robotid:
try:
answer = probs[0].SendCommand(
'RunCBiRRT psample 0.2 supportlinks 2 ' + footlinknames +
' smoothingitrs ' + str(normalsmoothingitrs) + ' ' +
TSRChainStringGrasping)
print "RunCBiRRT answer: ", str(answer)
except openrave_exception, e:
print "Cannot send command RunCBiRRT: "
print e
print arg1
print "arg2 : "
print arg2
print "arg3 : "
print arg3
sys.stdin.readline()
startik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' + footlinknames + ' movecog ' +arg1+ ' nummanips 2 maniptm 1 ' + arg2 + ' maniptm 2 ' + arg3 );
#startik = probs[0].SendCommand('DoGeneralIK exec nummanips 1 maniptm 4 '+arg2)
print "Got startik:"
print startik
print "------"
robotid.SetDOFValues(str2num(startik)) # Note: startik: T0_LH1 and T0_RH1
print "go to startik"
sys.stdin.readline()
T0_RH2 = T0_RH1
#T0_RH2 = Tee[1]
T0_RH2[0,3] -= 0.1
T0_RH2[1,3] -= 0.2
Rotate_RH2 = MakeTransform(matrix(xyz_rotation([0,-pi/4,-pi/3])),transpose(matrix([0,0,0])))
T0_RH2 = dot(T0_RH2,Rotate_RH2)
h2 = misc.DrawAxes(env,matrix(T0_RH2),1)
#T0_RH2[2,3] += 0.01
#arg1 = str(cogtarg).strip("[]")
self.robotid.WaitForController(0)
self.robotid.GetController().Reset(0)
self.robotid.SetDOFValues(self.rhandclosevals,self.rhanddofs)
self.robotid.SetDOFValues(self.lhandclosevals,self.lhanddofs)
if( self.StopAtKeyStrokes ):
print "Press Enter to plan startik --> goalik "
sys.stdin.readline()
# Get a trajectory from goalik to grasp configuration
goaljoints = deepcopy(goalik)
for i in range(TSRChainMimicDOF):
goaljoints += ' 0'
goaljoints = str2num(goaljoints)
try:
answer = probs[0].SendCommand('RunCBiRRT supportlinks 2 '+footlinknames+' smoothingitrs '+str(fastsmoothingitrs)+' jointgoals '+str(len(goaljoints))+' '+Serialize1DMatrix(matrix(goaljoints))+' '+TSRChainString)
print "RunCBiRRT answer: ",str(answer)
except openrave_exception, e:
print "Cannot send command RunCBiRRT: "
print e
try:
os.rename("cmovetraj.txt","movetraj1.txt")
except OSError, e:
# No file cmovetraj
print e
try:
# Went through all candidates
print "Found ",str(len(collisionFreeSolutions[0]))," valid solutions in ",str(howMany)," candidates."
if (len(collisionFreeSolutions[0]) > 0) :
# Have we found at least 1 valid path?
success = True
print "Press Enter to iterate through valid solutions."
sys.stdin.readline()
nxt = 0
ask = True
playAll = False
while(True):
print "Playing valid solution #: ",str(nxt)
robots[0].SetActiveDOFValues(str2num(collisionFreeSolutions[1][nxt]))
play(T0_starts, whereToFace, relBaseConstraint,candidates,numRobots,numManips,collisionFreeSolutions[0][nxt],myRmaps,robots,h,env,0.3,False,' leftFootBase rightFootBase ')
myCOM = array(get_robot_com(robots[0]))
myCOM[2,3] = 0.0
COMHandle = misc.DrawAxes(env,myCOM,0.3)
h.pop() # delete the robot base axis we added last
# Finally plan a trajectory from startik to goalik
startikStr = collisionFreeSolutions[1][nxt]
wheel.SetDOFValues([0],[0])
go_to_startik(robots[0], startikStr)
temp1 = MakeTransform(rodrigues([-pi/2,0,0]),transpose(matrix([0,0,0])))
temp2 = MakeTransform(rodrigues([0,0,-pi/2]),transpose(matrix([0,0,0])))
CTee = wheel.GetManipulators()[0].GetEndEffectorTransform()
def start(T0_starts,
T0_FACING,
candidates,
numRobots,
numManips,
c,
myRmaps,
robots,
h,
myEnv,
doGeneralIk,
footlinknames=''):
# Define this variable so python doesn't complain
myIK = ''
# constraints to check
masterBaseConstOK = True
# Move each robot / manipulators
for myRobotIndex in range(numRobots):
for myManipulatorIndex in range(numManips[myRobotIndex]):
# Find where to move the base
startSphereIndex = candidates[myManipulatorIndex][c][0].sIdx
startTransformIndex = candidates[myManipulatorIndex][c][0].tIdx
# Set the manipulator to its configuration (we will check for collision)
# print "for manipulator: ",str(myManipulatorIndex)
# print "go_to: start_sphere_index: "
# print startSphereIndex
# print "go_to: start_transform_index: "
# print startTransformIndex
myRmaps[myManipulatorIndex].go_to(startSphereIndex,
startTransformIndex)
Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[
startTransformIndex]
# for myT in myRmaps[myManipulatorIndex].map[startSphereIndex].T:
# print myT
T0_newManipPose = dot(T0_starts[myManipulatorIndex],
linalg.inv(Tbase_start))
# Finally move the robot base
robots[myRobotIndex].SetTransform(T0_newManipPose)
# print "manip index: ",str(myManipulatorIndex)
# sys.stdin.readline()
if (myManipulatorIndex == 0):
h.append(misc.DrawAxes(myEnv, T0_newManipPose, 0.4))
# Check master base constraint
# if(myRobotIndex == 0):
# if(type(masterBaseConstraint) == type([])):
# if((abs(T0_newManipPose[0:3,3].transpose()) > masterBaseConstraint[0:3]).any()):
# masterBaseConstOK = False
# break
# #if(not allclose(T0_newManipPose[0:3,0:3],rodrigues(masterBaseConstraint[3:6]))):
# # masterBaseConstOK = False
# # HACK-AROUND
# # For now just check if the robot base is on XY plane
# if((not allclose(T0_newManipPose[0:3,2].transpose(),[0,0,1])) or (not allclose(T0_newManipPose[2,0:3],[0,0,1]))):
# masterBaseConstOK = False
# break
# elif(type(masterBaseConstraint) == type(array(()))):
# pass
# Now check if the feet are on the ground
#
# TODO: We should do this in a robot-agnostic way.
# masterBaseConstraint works for mobile manipulators
# or for industrial arms, but for humanoids the base
# and the feet can be different, so for balance check
# we "may" need an extra step. How do we set this flag?
# Maybe we should have a "isHumanoid" bool? And try to
# put the feet on the ground if(myReachabilityMap.isHumanoid)
#
if (doGeneralIk):
# Reset the head and the legs to zero
robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0, 0.0],
[6, 7, 8, 9])
robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0], [24, 25, 29])
robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0], [30, 31, 35])
# Bend the knees to avoid singularity issues when solving GeneralIK
robots[myRobotIndex].SetDOFValues([-0.3, 0.6, -0.3], [32, 33, 34])
robots[myRobotIndex].SetDOFValues([-0.3, 0.6, -0.3], [26, 27, 28])
# print "trying to put the feet on the ground..."
if (footlinknames == ''):
myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING,
myEnv)
else:
myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING,
myEnv, footlinknames)
if (myIK != ''):
# if found a solution show it.
robots[myRobotIndex].SetActiveDOFValues(str2num(myIK))
else:
masterBaseConstOK = False
# Check collision with self and with the environment
if (myEnv.CheckCollision(robots[myRobotIndex])
or robots[myRobotIndex].CheckSelfCollision()):
collisionConstOK = False
else:
collisionConstOK = True
return [(masterBaseConstOK and collisionConstOK), myIK]
# print arg4
print "Press Enter to find a goalIK"
sys.stdin.readline()
crankid.SetDOFValues([crank_rot], [crankjointind])
goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' +
footlinknames + ' movecog ' + arg1 +
' nummanips 3 maniptm 0 ' + arg2 +
' maniptm 1 ' + arg3 + ' maniptm 2 ' + arg4)
# print "goalIK"
# print goalik
robotid.SetActiveDOFValues(str2num(goalik))
crankid.SetDOFValues([crank_rot], [crankjointind])
print "Press Enter to go to startik"
sys.stdin.readline()
# Get a trajectory from goalik to grasp configuration
goaljoints = deepcopy(goalik)
for i in range(TSRChainMimicDOF):
goaljoints += ' 0'
goaljoints = str2num(goaljoints)
robotid.SetActiveDOFValues(startik)
# Close hands to start "turning" the wheel
robotid.SetDOFValues(rhandclosevals, rhanddofs)
def test_points(self):
print "Testing points"
self.notPlanning = False # We are planning: this will stop robot model and wheel model being updated
manips = self.robot.GetManipulators()
crankmanip = self.crankid.GetManipulators()
print "Getting Loaded Problems"
self.probs = self.env.GetLoadedProblems()
print self.probs[0] # --> self.probs_cbirrt
print self.probs[1] # --> self.probs_crankmover
self.robot.SetActiveDOFs([15,16,17,18,19,20,21,27,28,29,30,31,32,33])
# InSyncWithROS sets DOF values for both OpenRAVE and Gazebo
self.SetRobotDOFValuesInSyncWithROS(self.robot,[-0.85,0.3,-1.00,-1.57,0.0,-1.00,-1.00,0.05],[27,28,29,30,31,32,33,34])
self.SetRobotDOFValuesInSyncWithROS(self.robot,[0.85, 0.3,1.00,-1.57,0.0,-1.00,1.00,0.05],[15,16,17,18,19,20,21,22])
self.initconfig = self.robot.GetActiveDOFValues()
print "initconfig"
print self.initconfig
print type(self.initconfig)
manip = self.robot.SetActiveManipulator('leftarm') # set the manipulator to leftarm
self.T0_LH_INIT = manip.GetEndEffectorTransform() # end of manipulator 7
manip = self.robot.SetActiveManipulator('rightarm') # set the manipulator to leftarm
self.T0_RH_INIT = manip.GetEndEffectorTransform() # end of manipulator 5
self.robot.SetActiveDOFs([15,16,17,18,19,20,21,27,28,29,30,31,32,33])
print "Press enter to continue 1..."
sys.stdin.readline()
links = self.robot.GetLinks()
self.crankjointind = 0
self.jointtm = self.probs[0].SendCommand('GetJointTransform name crank jointind '+str(self.crankjointind))
maniptm = self.crankid.GetManipulators()[0].GetTransform()
self.jointtm = self.jointtm.replace(" ",",")
self.jointtm = eval('['+self.jointtm+']')
rhanddofs = [34] # Right gripper links here
rhandclosevals = [0.035] # What are the closed joint values for the right hand's links?--> Obtained experimentally
rhandopenvals = [0.0548] # What are the open joint values for the right hand's links? --> Obtained experimentally
lhanddofs = [22] # Left gripper links here
lhandclosevals = [0.035] # Same as rhandclosevals for the left gripper --> Obtained experimentally
lhandopenvals = [0.0548] # Same as rhandopenvals for the left gripper --> Obtained experimentally
##############################################################
## THIS IS WHERE THE FIRST BLOCK ENDS IN THE MATLAB VERSION ##
##############################################################
self.robot.SetActiveDOFs([15,16,17,18,19,20,21,27,28,29,30,31,32,33])
# Rotate the driving wheel's manipulator's coordinate frame 90 degrees around its Z-axis
temp = dot(maniptm,MakeTransform(matrix(rodrigues([0,0,pi/2])),transpose(matrix([0,0,0]))))
# Rotate the new coordinate frame -90 degrees around its X-axis
temp = dot(temp,MakeTransform(matrix(rodrigues([-pi/2,0,0])),transpose(matrix([0,0,0]))))
# Rotate the new coordinate frame -30 degrees around its Y-axis
temp = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0,0]))))
# Translate the new coordinate frame -0.11 meters on its Z-axis and assign it to Left Hand
self.T0_LH1_APPROACH_1 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.1,0.0,-0.2]))))
self.T0_LH1_APPROACH_2 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.18]))))
self.T0_LH1_ENTRY = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.155]))))
# Rotate the driving wheel's manipulator's coordinate frame 90 degrees around its Z-axis
temp = dot(maniptm,MakeTransform(matrix(rodrigues([0,0,pi/2])),transpose(matrix([0,0,0]))))
# Rotate the new coordinate frame +90 degrees around its X-axis
temp = dot(temp,MakeTransform(matrix(rodrigues([pi/2,0,0])),transpose(matrix([0,0,0]))))
# Rotate the new coordinate frame 30 degrees around its Y-axis
temp = dot(temp,MakeTransform(matrix(rodrigues([0,pi/6,0])),transpose(matrix([0,0,0]))))
# Translate the new coordinate frame -0.11 meters on its Z-axis and assign it to Left Hand
self.T0_RH1_APPROACH_1 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.1,0.0,-0.2]))))
self.T0_RH1_APPROACH_2 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.18]))))
self.T0_RH1_ENTRY = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.155]))))
# debug: at this point, it can be useful to visualize where we want our hand to move (use visualization_msgs / markers?)
arg1 = str(GetRot(self.T0_LH1_APPROACH_1)).strip("[]")+str(GetTrans(self.T0_LH1_APPROACH_1)).strip("[]")
arg1 = arg1.replace("\n"," ")
arg2 = str(GetRot(self.T0_RH1_APPROACH_1)).strip("[]")+str(GetTrans(self.T0_RH1_APPROACH_1)).strip("[]")
arg2 = arg2.replace("\n"," ")
self.approachik_1 = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2)
print "Got self.approachik_1:"
if self.approachik_1 == '':
print "Hey: No IK Solution found! Move the robot!"
self.notPlanning = True
return []
else:
print str2num(self.approachik_1)
print type(str2num(self.approachik_1))
print "------"
self.robot.SetActiveDOFValues(str2num(self.approachik_1))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_1),self.robot.GetActiveDOFIndices())
print "went to approach-1"
sys.stdin.readline()
################################################################
## THIS IS WHERE THE SECOND BLOCK ENDS IN THE MATLAB VERSION ##
## APPROACH-1 IK IS OVER - NOW GO TO APPROACH-2 ##
################################################################
arg1 = str(GetRot(self.T0_LH1_APPROACH_2)).strip("[]")+str(GetTrans(self.T0_LH1_APPROACH_2)).strip("[]")
arg1 = arg1.replace("\n"," ")
arg2 = str(GetRot(self.T0_RH1_APPROACH_2)).strip("[]")+str(GetTrans(self.T0_RH1_APPROACH_2)).strip("[]")
arg2 = arg2.replace("\n"," ")
self.approachik_2 = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2)
print "Got approachik-2:"
if self.approachik_2 == '':
print "Hey: No IK Solution found! Move the robot!"
self.notPlanning = True
return []
else:
print self.approachik_2
print "------"
self.robot.SetActiveDOFValues(str2num(self.approachik_2))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_2),self.robot.GetActiveDOFIndices())
print "went to approach-2"
sys.stdin.readline()
####################################################################
## APPROACH-2 IK IS OVER - NOW GO TO STARTIK WHERE WE WILL GRASP ##
####################################################################
arg1 = str(GetRot(self.T0_LH1_ENTRY)).strip("[]")+str(GetTrans(self.T0_LH1_ENTRY)).strip("[]")
arg1 = arg1.replace("\n"," ")
arg2 = str(GetRot(self.T0_RH1_ENTRY)).strip("[]")+str(GetTrans(self.T0_RH1_ENTRY)).strip("[]")
arg2 = arg2.replace("\n"," ")
self.startik = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2)
print "Got self.startik:"
if self.startik == '':
print "Hey: No IK Solution found! Move the robot!"
self.notPlanning = True
return []
else:
print self.startik
print "------"
self.robot.SetActiveDOFValues(str2num(self.startik))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices())
print "went to startik"
sys.stdin.readline()
###################################################
## COLLISION FREE REACHING TO GRASP WITH CBiRRT ##
###################################################
## Now let's try a different version of the previous block.
## The previous block tries to go to initial configuration without any collision avoidance.
## IF we want to avoid colliding with the wheel, we need to define Task Space Regions:
# TSRString1 = SerializeTSR(5,'NULL',T0_RH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # RightHand_Torso manip: 4
# TSRString2 = SerializeTSR(7,'NULL',T0_LH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # LeftHand_Torso manip: 6
# #TSRChainStringGoToStartIK = SerializeTSRChain(0,1,0,1,TSRString1,'NULL',[])+' '+SerializeTSRChain(0,1,0,1,TSRString2,'NULL',[])
# TSRChainStringGoToStartIK = SerializeTSRChain(0,1,0,1,TSRString2,'NULL',[])
# #answer = probs[0].SendCommand('RunCBiRRT psample 0.2 smoothingitrs 150 %s'%(TSRChainStringGoToStartIK))
# answer = probs[0].SendCommand('RunCBiRRT psample 0.25 %s'%(TSRChainStringGoToStartIK))
# # Defined the TSRs as constraints during the trajectory execution. Now let's send them over to the planner.
# print answer
# if (answer == 1):
# print "runcbirrt successful for finding a path to start ik without collision with the wheel"
# startik = self.robot.GetDOFValues()
# print "Got startik:"
# print startik
# print "------"
# # Rename the file so that we can keep the data w/o overwriting it with a new trajectory
# try:
# os.rename("cmovetraj.txt","movetraj-goto-startik.txt")
# if os.path.isfile('movetraj-goto-startik.txt'):
# try:
# answer = pr2_rave2task_control('movetraj-goto-startikt.txt')
# except openrave_exception, e:
# print e
# self.robot.WaitForController(0)
# print "Press enter to restart"
# sys.stdin.readline()
# else:
# print "Can't find movetraj-goto-startik.txt"
# except OSError, e:
# print e
# print "went to startik"
# sys.stdin.readline()
# else:
# print "Hey: No IK Solution found! Move the robot!"
# print T0_LH1
# print T0_RH1
# self.notPlanning = True
# return []
##############################################################
## END OF STARTIK TSR DEFINITION AND TRAJECTORY EXEC. BLOCK ##
##############################################################
# Left hand is evaluated first, so need to make right hand relative to crank
cranklinks = self.crankid.GetLinks();
self.T0_crankcrank = self.crankid.GetManipulators()[0].GetTransform()
T0_w0L = eye(4)
T0_w0R = MakeTransform(rodrigues([0,self.wristPitch,0]),matrix([[self.jointtm[9]],[self.jointtm[10]],[self.jointtm[11]]]))
crank_rot = pi/6
TSRChainMimicDOF = 1
Tcrank_rot = MakeTransform(matrix(rodrigues([crank_rot,0,0])),transpose(matrix([0,0,0]))) # For the right gripper
Tcrank_rot2 = MakeTransform(matrix(rodrigues([0,0,crank_rot])),transpose(matrix([0,0,0]))); # For the left gripper
T0_cranknew = dot(T0_w0R,Tcrank_rot)
temp = dot(linalg.inv(T0_w0R),self.T0_RH1_ENTRY)
T0_RH2 = dot(T0_cranknew,temp);
temp = dot(linalg.inv(self.T0_crankcrank),self.T0_LH1_ENTRY)
temp = dot(Tcrank_rot2,temp)
T0_LH2 = dot(self.T0_crankcrank,temp)
arg1 = str(GetRot(T0_LH2)).strip("[]")+str(GetTrans(T0_LH2)).strip("[]")
arg2 = str(GetRot(T0_RH2)).strip("[]")+str(GetTrans(T0_RH2)).strip("[]")
self.crankid.SetDOFValues([crank_rot],[self.crankjointind])
# Get the goal inverse kinematics
self.goalik = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2)
print "Got self.goalik:"
if self.startik == '':
print "Hey: No IK Solution found! Change the target!"
self.notPlanning = True
return []
else:
print self.goalik
print "------"
#self.robot.SetActiveDOFValues(str2num(goalik)) # Note: self.goalik is T0_LH2 and T0_RH2
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.goalik),self.robot.GetActiveDOFIndices())
print "went to goalik"
sys.stdin.readline()
# Let the TSR Magic Happen
self.crankid.SetDOFValues([crank_rot],[self.crankjointind])
#########################################################
# This is the end of the second block in Matlab version #
#########################################################
self.crankid.SetDOFValues([self.crankjointind])
#self.robot.SetActiveDOFValues(str2num(startik))
self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices())
################################################
## YOU'RE AT STARTIK - GO BACK TO APPROACH 2 ##
################################################
goaljoints = str2num(self.approachik_2);
print "Press enter to go to approach point 2"
sys.stdin.readline()
self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices())
##################################################
## YOU'RE AT APPROACH 2 - GO BACK TO APPROACH 1 ##
##################################################
goaljoints = str2num(self.approachik_1);
print "Press enter to go to approach point 1"
sys.stdin.readline()
self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices())
######################################################
## YOU'RE AT APPROACH 1 - GO BACK TO INITCONFIG ##
######################################################
print "Press enter to go to initconfig"
sys.stdin.readline()
self.SetRobotDOFValuesInSyncWithROS(self.robot,self.initconfig,self.robot.GetActiveDOFIndices())
return 1
def start(T0_starts, T0_FACING, candidates,numRobots,numManips,c,myRmaps,robots,h,myEnv,doGeneralIk,footlinknames=''):
# Define this variable so python doesn't complain
myIK = ''
# constraints to check
masterBaseConstOK = True
# Move each robot / manipulators
for myRobotIndex in range(numRobots):
for myManipulatorIndex in range(numManips[myRobotIndex]):
# Find where to move the base
startSphereIndex = candidates[myManipulatorIndex][c][0].sIdx
startTransformIndex = candidates[myManipulatorIndex][c][0].tIdx
# Set the manipulator to its configuration (we will check for collision)
# print "for manipulator: ",str(myManipulatorIndex)
# print "go_to: start_sphere_index: "
# print startSphereIndex
# print "go_to: start_transform_index: "
# print startTransformIndex
myRmaps[myManipulatorIndex].go_to(startSphereIndex,startTransformIndex)
Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[startTransformIndex]
# for myT in myRmaps[myManipulatorIndex].map[startSphereIndex].T:
# print myT
T0_newManipPose = dot(T0_starts[myManipulatorIndex],linalg.inv(Tbase_start))
# Finally move the robot base
robots[myRobotIndex].SetTransform(T0_newManipPose)
# print "manip index: ",str(myManipulatorIndex)
# sys.stdin.readline()
if(myManipulatorIndex == 0):
h.append(misc.DrawAxes(myEnv,T0_newManipPose,0.4))
# Check master base constraint
# if(myRobotIndex == 0):
# if(type(masterBaseConstraint) == type([])):
# if((abs(T0_newManipPose[0:3,3].transpose()) > masterBaseConstraint[0:3]).any()):
# masterBaseConstOK = False
# break
# #if(not allclose(T0_newManipPose[0:3,0:3],rodrigues(masterBaseConstraint[3:6]))):
# # masterBaseConstOK = False
# # HACK-AROUND
# # For now just check if the robot base is on XY plane
# if((not allclose(T0_newManipPose[0:3,2].transpose(),[0,0,1])) or (not allclose(T0_newManipPose[2,0:3],[0,0,1]))):
# masterBaseConstOK = False
# break
# elif(type(masterBaseConstraint) == type(array(()))):
# pass
# Now check if the feet are on the ground
#
# TODO: We should do this in a robot-agnostic way.
# masterBaseConstraint works for mobile manipulators
# or for industrial arms, but for humanoids the base
# and the feet can be different, so for balance check
# we "may" need an extra step. How do we set this flag?
# Maybe we should have a "isHumanoid" bool? And try to
# put the feet on the ground if(myReachabilityMap.isHumanoid)
#
if(doGeneralIk):
# Reset the head and the legs to zero
robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0, 0.0],[6,7,8,9])
robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0],[24,25,29])
robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0],[30,31,35])
# Bend the knees to avoid singularity issues when solving GeneralIK
robots[myRobotIndex].SetDOFValues([-0.3,0.6,-0.3],[32,33,34])
robots[myRobotIndex].SetDOFValues([-0.3,0.6,-0.3],[26,27,28])
# print "trying to put the feet on the ground..."
if(footlinknames==''):
myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv)
else:
myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv, footlinknames)
if(myIK != ''):
# if found a solution show it.
robots[myRobotIndex].SetActiveDOFValues(str2num(myIK))
else:
masterBaseConstOK = False
# Check collision with self and with the environment
if(myEnv.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()):
collisionConstOK = False
else:
collisionConstOK = True
return [(masterBaseConstOK and collisionConstOK), myIK]
handtrans1 = T0_object[0:3, 3] + transoffset1
handrot1 = mat(rodrigues([pi / 2, 0, 0]))
Tik0 = MakeTransform(handrot0, handtrans0)
Tik1 = MakeTransform(handrot1, handtrans1)
#get the ik solutions for both arms for the box in the start pose
robot.SetActiveDOFs(arm0dofs)
startik0 = probs_cbirrt.SendCommand(
'DoGeneralIK exec nummanips 1 maniptm 0 %s' % SerializeTransform(Tik0))
robot.SetActiveDOFs(arm1dofs)
startik1 = probs_cbirrt.SendCommand(
'DoGeneralIK exec nummanips 1 maniptm 1 %s' % SerializeTransform(Tik1))
startik = '%s %s' % (startik0, startik1)
robot.SetActiveDOFs(activedofs)
robot.SetActiveDOFValues(str2num(startik))
#define the target transform of the goal
T0_object2 = MakeTransform(mat(eye(3)), mat([0.6923, 0.00, 1.3989]).T)
targobject.SetTransform(array(T0_object2[0:3][:, 0:4]))
#define targets for both hands relative to box in goal pose
handtrans0 = T0_object2[0:3, 3] + transoffset0
handtrans1 = T0_object2[0:3, 3] + transoffset1
Tik0 = MakeTransform(handrot0, handtrans0)
Tik1 = MakeTransform(handrot1, handtrans1)
#get the ik solutions for both arms for the box in the goal pose
robot.SetActiveDOFs(arm0dofs)
goalik0 = probs_cbirrt.SendCommand(
'DoGeneralIK exec nummanips 1 maniptm 0 %s' % SerializeTransform(Tik0))
def play(T0_starts, T0_FACING, relBaseConstraint,candidates,numRobots,numManips,c,myRmaps,robots,h,myEnv,howLong,doGeneralIk,footlinknames=''):
# constraints to check
relBaseConstOK = True
collisionConstOK = True
noConfigJump = True
# Get the length of the candidate path
# First index stands for the robot index, and the second index stands for the candidate path index. We're calling find_random_candidates() function with an argument of 1. Thus there will be only one candidate returned. Let's find it's length.
pathLength = min(len(candidates[0][c]), len(candidates[1][c]))
# print "numRobots: ",str(numRobots)
# print "numManips: ",str(numManips)
[goAhead, activeDOFStartConfig] = start(T0_starts,T0_FACING, candidates,numRobots,numManips,c,myRmaps,robots,h,myEnv,doGeneralIk,footlinknames)
if(goAhead):
# Get their relative Transformation matrix
T0_base = []
for myRobotIndex in range(numRobots):
for myManipulatorIndex in range(numManips[myRobotIndex]):
T0_base.append(robots[myRobotIndex].GetManipulators()[myManipulatorIndex].GetBase().GetTransform())
Trobot0_robot1 = dot(linalg.inv(T0_base[0]),T0_base[1])
# i) Does the candidate satisfy the robot base transform constraint?
if(type(relBaseConstraint) == type([])):
# if the input argument type is a list then we have bounds
# Check translation constraints
if((abs(Trobot0_robot1[0:3,3].transpose()) > relBaseConstraint[0:3]).any()):
relBaseConstOK = False
return [False, '']
# Check rotation constraints
if(not allclose(Trobot0_robot1[0:3,0:3],rodrigues(relBaseConstraint[3:6]))):
relBaseConstOK = False
return [False, '']
elif(type(relBaseConstraint) == type(array(()) or relBaseConstraint) == type(matrix(()))):
# if input argument type is a numpy array then we have an exact transformation
#print "relBaseConstraint?"
#print allclose(Trobot0_robot1,relBaseConstraint)
if(not allclose(Trobot0_robot1,relBaseConstraint)):
relBaseConstOK = False
return [False, '']
# If the solution meets the base constraint:
if(relBaseConstOK):
# print "Base Constraints OK."
pathConfigs = [[],[]] # A 2D List
# ii) Check if the solution collision-free throughout the path?
# For each path element, go step by step and check
prevConfig = [[],[]]
currentConfig = [[],[]]
# print "----"
for pElementIndex in range(pathLength):
# Move the manipulator to this path element
for myRobotIndex in range(numRobots):
for myManipulatorIndex in range(numManips[myRobotIndex]):
currentSphereIndex = candidates[myManipulatorIndex][c][pElementIndex].sIdx
currentTransformIndex = candidates[myManipulatorIndex][c][pElementIndex].tIdx
myRmaps[myManipulatorIndex].go_to(currentSphereIndex,currentTransformIndex)
pathConfigs[myManipulatorIndex].append(robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[myManipulatorIndex].GetArmIndices()))
# DEBUG SECTION FOR SENSING CONFIGURATION JUMP
currentConfig[myManipulatorIndex] = robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[myManipulatorIndex].GetArmIndices())
# print "For robot ",str(myRobotIndex)," ||qA-qB||:"
# print "path element ",str(pElementIndex)
if(prevConfig[myManipulatorIndex] != []):
# print "previous config: "
# print prevConfig[myRobotIndex]
# print "current config: "
# print currentConfig[myRobotIndex]
qdiff = absolute(subtract(currentConfig[myManipulatorIndex],prevConfig[myManipulatorIndex]))
configDistSq = 0
# for each joint do:
for j in range(len(qdiff)):
configDistSq += pow(qdiff[j],2)
# find ||qA-qB||
euclideanConfigDistance = pow(configDistSq,0.5)
# print "euclidean configuration distance: "
# print euclideanConfigDistance
# if(euclideanConfigDistance > configurationJumpThreshold):
# noConfigJump = False
# return False
else:
#print "skipping path element:"
pass
# END OF DEBUG SECTION FOR SENSING CONFIGURATION JUMP
prevConfig[myManipulatorIndex] = deepcopy(currentConfig[myManipulatorIndex])
# Check collision with self and with the environment
if(myEnv.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()):
collisionConstOK = False
return [False, '']
# After setting manipulator configurations for this
# robot, check if the center of mass is close enough
# to the center of robot's feet
#
# TODO: This part is very messy and ugly.
# I need to clean it up and make nice function calls.
if(doGeneralIk):
if(footlinknames==''):
myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv)
else:
myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv, footlinknames)
if(myIK != ''):
# if successful, show it
robots[myRobotIndex].SetActiveDOFValues(str2num(myIK))
# print "checking support in play..."
if(not check_support(array(get_robot_com(robots[myRobotIndex])),robots[myRobotIndex])):
return [False, '']
else:
pass
# print "in balance - path element: ",str(pElementIndex)
#sys.stdin.readline()
else:
return [False, '']
# If you didn't break yet, wait before the next path element for visualization
time.sleep(howLong)
for manipIdx, manipConfs in enumerate(pathConfigs):
# print "for manip ",str(manipIdx)
for confIdx, conf in enumerate(manipConfs):
if(confIdx > 0):
qdiff = absolute(subtract(conf,manipConfs[confIdx-1]))
confDistSq = 0
for j in range(len(qdiff)):
configDistSq += pow(qdiff[j],2)
eConfDist = pow(configDistSq,0.5)
# print "euclidean configuration distance:"
# print eConfDist
if(eConfDist > configurationJumpThreshold):
noConfigJump = False
return [False, '']
# If you made it here,
# it means no configuration jump, no collision, and
# the center of mass was close enough to the center
# of robot's feet for all reachability spheres.
return [True, activeDOFStartConfig]
else:
# start() failed
return [False, '']
def go_to_startik(myRobot, startik):
# print "Went to startik"
myRobot.SetActiveDOFValues(str2num(startik))
time.sleep(0.05)
if(debug):
sys.stdin.readline()
myCBiRRTProblem = RaveCreateModule(myEnv,'CBiRRT')
myManipulationProblem = RaveCreateModule(myEnv,'Manipulation')
try:
myEnv.AddModule(myCBiRRTProblem,myRobot.GetName()) # this string should match to <Robot name="" > in robot.xml
myEnv.AddModule(myManipulationProblem,myRobot.GetName()) # this string should match to <Robot name="" > in robot.xml
except openrave_exception, e:
print e
#grab the object with manipulator 0
myManipulationProblem.SendCommand('setactivemanip index 0')
myManipulationProblem.SendCommand('GrabBody name '+kinBodyToGrab)
jointgoalsStr = deepcopy(goalikStr)
jointgoalsNum = str2num(jointgoalsStr)
try:
# answer = myCBiRRTProblem.SendCommand('RunCBiRRT timelimit 5 smoothingitrs 1 jointgoals %s %s %s'%(len(jointgoalsNum),Serialize1DMatrix(mat(jointgoalsNum)),TSRChainString))
answer = myCBiRRTProblem.SendCommand('RunCBiRRT timelimit 5 supportlinks 2 '+footlinknames+' smoothingitrs '+str(fastsmoothingitrs)+' jointgoals '+str(len(jointgoalsNum))+' '+Serialize1DMatrix(matrix(jointgoalsNum))+' '+TSRChainString)
print
print "RunCBiRRT answer: ",str(answer)
except openrave_exception, e:
print "Cannot send command RunCBiRRT: "
print e
# cleanup the cbirrt problem object
myEnv.Remove(myCBiRRTProblem)
myEnv.Remove(myManipulationProblem)
print "arg3 : "
print arg3
sys.stdin.readline()
startik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' +
footlinknames + ' movecog ' + arg1 +
' nummanips 2 maniptm 1 ' + arg2 +
' maniptm 2 ' + arg3)
#startik = probs[0].SendCommand('DoGeneralIK exec nummanips 1 maniptm 4 '+arg2)
print "Got startik:"
print startik
print "------"
robotid.SetDOFValues(str2num(startik)) # Note: startik: T0_LH1 and T0_RH1
print "go to startik"
sys.stdin.readline()
T0_RH2 = T0_RH1
#T0_RH2 = Tee[1]
T0_RH2[0, 3] -= 0.1
T0_RH2[1, 3] -= 0.2
Rotate_RH2 = MakeTransform(matrix(xyz_rotation([0, -pi / 4, -pi / 3])),
transpose(matrix([0, 0, 0])))
T0_RH2 = dot(T0_RH2, Rotate_RH2)
h2 = misc.DrawAxes(env, matrix(T0_RH2), 1)
#T0_RH2[2,3] += 0.01
def go_to_startik(myRobot, startik):
# print "Went to startik"
myRobot.SetActiveDOFValues(str2num(startik))
time.sleep(0.05)
if (debug):
sys.stdin.readline()
# Try to put your feet on the ground
print "trying to put the feet on the ground..."
whereToFace = MakeTransform(rodrigues([0, 0, -pi / 2]),
transpose(matrix([0, 0, 0])))
myIK = put_feet_on_the_ground(robots[0], whereToFace, lowerLimits,
upperLimits, env)
print "myIK"
print myIK
if (myIK != ''):
print "checking balance constraint..."
# print "Press enter to see the result..."
# sys.stdin.readline()
robots[0].SetDOFValues(str2num(myIK),
range(len(robots[0].GetJoints())))
myCOM = array(get_robot_com(robots[0]))
myCOM[2, 3] = 0.0
COMHandle = misc.DrawAxes(env, myCOM, 0.3)
if (check_support(myCOM, robots[0])):
print "This solution is valid"
print "left hand path [s:t]: "
for pe in candidates[0][collisionFreeSolutions[nxt]]:
print str(pe.sIdx), ' : ', str(pe.tIdx)
print "right hand path [s:t]: "
for pe in candidates[1][collisionFreeSolutions[nxt]]:
print str(pe.sIdx), ' : ', str(pe.tIdx)
print "Press enter to proceed with the next solution..."
sys.stdin.readline()
currentIk = robots[0].GetDOFValues()
print currentIk
# Try to put your feet on the ground
print "trying to put the feet on the ground..."
whereToFace = MakeTransform(rodrigues([0, 0, -pi / 2]), transpose(matrix([0, 0, 0])))
myIK = put_feet_on_the_ground(robots[0], whereToFace, lowerLimits, upperLimits, env)
print "myIK"
print myIK
if myIK != "":
print "checking balance constraint..."
# print "Press enter to see the result..."
# sys.stdin.readline()
robots[0].SetDOFValues(str2num(myIK), range(len(robots[0].GetJoints())))
myCOM = array(get_robot_com(robots[0]))
myCOM[2, 3] = 0.0
COMHandle = misc.DrawAxes(env, myCOM, 0.3)
if check_support(myCOM, robots[0]):
print "This solution is valid"
print "left hand path [s:t]: "
for pe in candidates[0][collisionFreeSolutions[nxt]]:
print str(pe.sIdx), " : ", str(pe.tIdx)
print "right hand path [s:t]: "
for pe in candidates[1][collisionFreeSolutions[nxt]]:
print str(pe.sIdx), " : ", str(pe.tIdx)
print "Press enter to proceed with the next solution..."
sys.stdin.readline()
else:
# Clean the trajectory files so that we don't execute an old trajectory
for i in range(10):
try:
print "Removing movetraj"+str(i)+".txt"
os.remove("movetraj"+str(i)+".txt")
except OSError, e:
print e
#######################################
## CREATE TRAJ 0: INIT-->APPROACH 1 ##
#######################################
print "Press enter to generate init --> approach-1 (trajectory0)"
sys.stdin.readline()
goaljoints = str2num(self.approachik_1);
TSRString_A1_R = SerializeTSR(5,'NULL',self.T0_RH1_APPROACH_1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
TSRString_A1_L = SerializeTSR(7,'NULL',self.T0_LH1_APPROACH_1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
TSRChainString_A1 = SerializeTSRChain(0,0,1,1,TSRString_A1_R,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRString_A1_L,'NULL',[])
try:
answer = self.probs[0].SendCommand('RunCBiRRT jointgoals %d %s %s'%(len(goaljoints),Serialize1DMatrix(matrix(goaljoints)),TSRChainString_A1))
print "runcbirrt successful"
print answer
if (answer == '0'):
return []
except openrave_exception, e:
print "Cannot send command runcbirrt"
print e
# Rename the file so that we can keep the data w/o overwriting it with a new trajectory
def run(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm, T0_LH, T0_RH):
# Search for the pattern in the reachability model and get the results
numRobots = 1
# Keep the number of manipulators of the robots
# that are going to be involved in search()
# The length of this list should be the same with
# the number of robots involved.
numManips = [2]
T0_p = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))
myPatterns = []
# 2. Create a search pattern from the trajectory for the first manipulator
myPattern = SearchPattern(leftTraj)
myPattern.T0_p = T0_p
myPattern.setColor(array((0,0,1,0.5)))
myPattern.show(env)
# sys.stdin.readline()
myPattern.hide("all")
myPatterns.append(myPattern)
# 3. Create a search pattern from the trajectory for the second manipulator
myPattern = SearchPattern(rightTraj)
myPattern.T0_p = T0_p
myPattern.setColor(array((1,0,0,0.5)))
myPattern.show(env)
# sys.stdin.readline()
myPattern.hide("all")
myPatterns.append(myPattern)
for p in myPatterns:
p.hide("spheres")
#sys.stdin.readline()
for p in myPatterns:
p.hide("all")
# sys.stdin.readline()
T0_starts = []
T0_starts.append(array(T0_LH))
T0_starts.append(array(T0_RH))
mySamples = []
candidates = None
print "Ready to look for candidates... ",str(datetime.now())
candidates = look_for_candidates(pairs, rm, myPatterns)
if(candidates != None):
# find how many candidates search() function found.
# candidates[0] is the list of candidate paths for the 0th robot
howMany = len(candidates[0])
collisionFreeSolutions = []
valids = []
for c in range(howMany):
print "trying ",str(c)," of ",str(howMany)," candidates."
allGood = play(T0_starts, relBaseConstraint,candidates,numRobots,numManips,c,myRmaps,[robot],h,env,0.0)
h.pop() # delete the robot base axis we added last
# We went through all our constraints. Is the candidate valid?
if(allGood):
collisionFreeSolutions.append(c)
findAPathEnds = time.time()
print "Success! Collision and configuration constraints met."
else:
print "Constraint(s) not met ."
# Went through all candidates
print "Found ",str(len(collisionFreeSolutions))," collision-free solutions in ",str(howMany)," candidates."
if (len(collisionFreeSolutions) > 0) :
# Have we found at least 1 collision free path?
for colFreeSolIdx, solIdx in enumerate(collisionFreeSolutions):
print "Playing valid solution #: ",str(colFreeSolIdx)
play(T0_starts, relBaseConstraint,candidates,numRobots,numManips,solIdx,myRmaps,[robot],h,env,0.3)
h.pop() # delete the robot base axis we added last
currentIk = robot.GetDOFValues()
# Try to put your feet on the ground
print "trying to put the feet on the ground..."
whereToFace = MakeTransform(rodrigues([0,0,0]),transpose(matrix([0,0,0])))
myIK = put_feet_on_the_ground(myProblem, robot, whereToFace, lowerLimits, upperLimits, env)
print "myIK"
print myIK
if(myIK != ''):
print "checking balance constraint..."
robot.SetDOFValues(str2num(myIK), range(len(robot.GetJoints())))
myCOM = array(get_robot_com(robot))
myCOM[2,3] = 0.0
COMHandle = misc.DrawAxes(env,myCOM,0.3)
if(check_support(myCOM,robot)):
mySamples.append([candidates[0][solIdx],candidates[1][solIdx]])
else:
print "COM is out of the support polygon"
robot.SetDOFValues(currentIk, range(len(robot.GetJoints())))
return mySamples
myManipulationProblem = RaveCreateModule(myEnv, 'Manipulation')
try:
myEnv.AddModule(myCBiRRTProblem, myRobot.GetName(
)) # this string should match to <Robot name="" > in robot.xml
myEnv.AddModule(myManipulationProblem, myRobot.GetName(
)) # this string should match to <Robot name="" > in robot.xml
except openrave_exception, e:
print e
#grab the object with manipulator 0
myManipulationProblem.SendCommand('setactivemanip index 0')
myManipulationProblem.SendCommand('GrabBody name ' + kinBodyToGrab)
jointgoalsStr = deepcopy(goalikStr)
jointgoalsNum = str2num(jointgoalsStr)
try:
# answer = myCBiRRTProblem.SendCommand('RunCBiRRT timelimit 5 smoothingitrs 1 jointgoals %s %s %s'%(len(jointgoalsNum),Serialize1DMatrix(mat(jointgoalsNum)),TSRChainString))
answer = myCBiRRTProblem.SendCommand(
'RunCBiRRT timelimit 5 supportlinks 2 ' + footlinknames +
' smoothingitrs ' + str(fastsmoothingitrs) + ' jointgoals ' +
str(len(jointgoalsNum)) + ' ' +
Serialize1DMatrix(matrix(jointgoalsNum)) + ' ' + TSRChainString)
print
print "RunCBiRRT answer: ", str(answer)
except openrave_exception, e:
print "Cannot send command RunCBiRRT: "
print e
def run(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm, T0_LH,
T0_RH):
# Search for the pattern in the reachability model and get the results
numRobots = 1
# Keep the number of manipulators of the robots
# that are going to be involved in search()
# The length of this list should be the same with
# the number of robots involved.
numManips = [2]
T0_p = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))
myPatterns = []
# 2. Create a search pattern from the trajectory for the first manipulator
myPattern = SearchPattern(leftTraj)
myPattern.T0_p = T0_p
myPattern.setColor(array((0, 0, 1, 0.5)))
myPattern.show(env)
# sys.stdin.readline()
myPattern.hide("all")
myPatterns.append(myPattern)
# 3. Create a search pattern from the trajectory for the second manipulator
myPattern = SearchPattern(rightTraj)
myPattern.T0_p = T0_p
myPattern.setColor(array((1, 0, 0, 0.5)))
myPattern.show(env)
# sys.stdin.readline()
myPattern.hide("all")
myPatterns.append(myPattern)
for p in myPatterns:
p.hide("spheres")
#sys.stdin.readline()
for p in myPatterns:
p.hide("all")
# sys.stdin.readline()
T0_starts = []
T0_starts.append(array(T0_LH))
T0_starts.append(array(T0_RH))
mySamples = []
candidates = None
print "Ready to look for candidates... ", str(datetime.now())
candidates = look_for_candidates(pairs, rm, myPatterns)
if (candidates != None):
# find how many candidates search() function found.
# candidates[0] is the list of candidate paths for the 0th robot
howMany = len(candidates[0])
collisionFreeSolutions = []
valids = []
for c in range(howMany):
print "trying ", str(c), " of ", str(howMany), " candidates."
allGood = play(T0_starts, relBaseConstraint, candidates, numRobots,
numManips, c, myRmaps, [robot], h, env, 0.0)
h.pop() # delete the robot base axis we added last
# We went through all our constraints. Is the candidate valid?
if (allGood):
collisionFreeSolutions.append(c)
findAPathEnds = time.time()
print "Success! Collision and configuration constraints met."
else:
print "Constraint(s) not met ."
# Went through all candidates
print "Found ", str(
len(collisionFreeSolutions)), " collision-free solutions in ", str(
howMany), " candidates."
if (len(collisionFreeSolutions) > 0):
# Have we found at least 1 collision free path?
for colFreeSolIdx, solIdx in enumerate(collisionFreeSolutions):
print "Playing valid solution #: ", str(colFreeSolIdx)
play(T0_starts, relBaseConstraint, candidates, numRobots,
numManips, solIdx, myRmaps, [robot], h, env, 0.3)
h.pop() # delete the robot base axis we added last
currentIk = robot.GetDOFValues()
# Try to put your feet on the ground
print "trying to put the feet on the ground..."
whereToFace = MakeTransform(rodrigues([0, 0, 0]),
transpose(matrix([0, 0, 0])))
myIK = put_feet_on_the_ground(myProblem, robot, whereToFace,
lowerLimits, upperLimits, env)
print "myIK"
print myIK
if (myIK != ''):
print "checking balance constraint..."
robot.SetDOFValues(str2num(myIK),
range(len(robot.GetJoints())))
myCOM = array(get_robot_com(robot))
myCOM[2, 3] = 0.0
COMHandle = misc.DrawAxes(env, myCOM, 0.3)
if (check_support(myCOM, robot)):
mySamples.append(
[candidates[0][solIdx], candidates[1][solIdx]])
else:
print "COM is out of the support polygon"
robot.SetDOFValues(currentIk, range(len(robot.GetJoints())))
return mySamples
def play(T0_starts,
T0_FACING,
relBaseConstraint,
candidates,
numRobots,
numManips,
c,
myRmaps,
robots,
h,
myEnv,
howLong,
doGeneralIk,
footlinknames=''):
# constraints to check
relBaseConstOK = True
collisionConstOK = True
noConfigJump = True
# Get the length of the candidate path
# First index stands for the robot index, and the second index stands for the candidate path index. We're calling find_random_candidates() function with an argument of 1. Thus there will be only one candidate returned. Let's find it's length.
pathLength = min(len(candidates[0][c]), len(candidates[1][c]))
# print "numRobots: ",str(numRobots)
# print "numManips: ",str(numManips)
[goAhead,
activeDOFStartConfig] = start(T0_starts, T0_FACING, candidates, numRobots,
numManips, c, myRmaps, robots, h, myEnv,
doGeneralIk, footlinknames)
if (goAhead):
# Get their relative Transformation matrix
T0_base = []
for myRobotIndex in range(numRobots):
for myManipulatorIndex in range(numManips[myRobotIndex]):
T0_base.append(robots[myRobotIndex].GetManipulators()
[myManipulatorIndex].GetBase().GetTransform())
Trobot0_robot1 = dot(linalg.inv(T0_base[0]), T0_base[1])
# i) Does the candidate satisfy the robot base transform constraint?
if (type(relBaseConstraint) == type([])):
# if the input argument type is a list then we have bounds
# Check translation constraints
if ((abs(Trobot0_robot1[0:3, 3].transpose()) >
relBaseConstraint[0:3]).any()):
relBaseConstOK = False
return [False, '']
# Check rotation constraints
if (not allclose(Trobot0_robot1[0:3, 0:3],
rodrigues(relBaseConstraint[3:6]))):
relBaseConstOK = False
return [False, '']
elif (type(relBaseConstraint) == type(array(
()) or relBaseConstraint) == type(matrix(()))):
# if input argument type is a numpy array then we have an exact transformation
#print "relBaseConstraint?"
#print allclose(Trobot0_robot1,relBaseConstraint)
if (not allclose(Trobot0_robot1, relBaseConstraint)):
relBaseConstOK = False
return [False, '']
# If the solution meets the base constraint:
if (relBaseConstOK):
# print "Base Constraints OK."
pathConfigs = [[], []] # A 2D List
# ii) Check if the solution collision-free throughout the path?
# For each path element, go step by step and check
prevConfig = [[], []]
currentConfig = [[], []]
# print "----"
for pElementIndex in range(pathLength):
# Move the manipulator to this path element
for myRobotIndex in range(numRobots):
for myManipulatorIndex in range(numManips[myRobotIndex]):
currentSphereIndex = candidates[myManipulatorIndex][c][
pElementIndex].sIdx
currentTransformIndex = candidates[myManipulatorIndex][
c][pElementIndex].tIdx
myRmaps[myManipulatorIndex].go_to(
currentSphereIndex, currentTransformIndex)
pathConfigs[myManipulatorIndex].append(
robots[myRobotIndex].GetDOFValues(
robots[myRobotIndex].GetManipulators()
[myManipulatorIndex].GetArmIndices()))
# DEBUG SECTION FOR SENSING CONFIGURATION JUMP
currentConfig[myManipulatorIndex] = robots[
myRobotIndex].GetDOFValues(
robots[myRobotIndex].GetManipulators()
[myManipulatorIndex].GetArmIndices())
# print "For robot ",str(myRobotIndex)," ||qA-qB||:"
# print "path element ",str(pElementIndex)
if (prevConfig[myManipulatorIndex] != []):
# print "previous config: "
# print prevConfig[myRobotIndex]
# print "current config: "
# print currentConfig[myRobotIndex]
qdiff = absolute(
subtract(currentConfig[myManipulatorIndex],
prevConfig[myManipulatorIndex]))
configDistSq = 0
# for each joint do:
for j in range(len(qdiff)):
configDistSq += pow(qdiff[j], 2)
# find ||qA-qB||
euclideanConfigDistance = pow(configDistSq, 0.5)
# print "euclidean configuration distance: "
# print euclideanConfigDistance
# if(euclideanConfigDistance > configurationJumpThreshold):
# noConfigJump = False
# return False
else:
#print "skipping path element:"
pass
# END OF DEBUG SECTION FOR SENSING CONFIGURATION JUMP
prevConfig[myManipulatorIndex] = deepcopy(
currentConfig[myManipulatorIndex])
# Check collision with self and with the environment
if (myEnv.CheckCollision(robots[myRobotIndex])
or robots[myRobotIndex].CheckSelfCollision()):
collisionConstOK = False
return [False, '']
# After setting manipulator configurations for this
# robot, check if the center of mass is close enough
# to the center of robot's feet
#
# TODO: This part is very messy and ugly.
# I need to clean it up and make nice function calls.
if (doGeneralIk):
if (footlinknames == ''):
myIK = put_feet_on_the_ground(
robots[myRobotIndex], T0_FACING, myEnv)
else:
myIK = put_feet_on_the_ground(
robots[myRobotIndex], T0_FACING, myEnv,
footlinknames)
if (myIK != ''):
# if successful, show it
robots[myRobotIndex].SetActiveDOFValues(
str2num(myIK))
# print "checking support in play..."
if (not check_support(
array(get_robot_com(robots[myRobotIndex])),
robots[myRobotIndex])):
return [False, '']
else:
pass
# print "in balance - path element: ",str(pElementIndex)
#sys.stdin.readline()
else:
return [False, '']
# If you didn't break yet, wait before the next path element for visualization
time.sleep(howLong)
for manipIdx, manipConfs in enumerate(pathConfigs):
# print "for manip ",str(manipIdx)
for confIdx, conf in enumerate(manipConfs):
if (confIdx > 0):
qdiff = absolute(
subtract(conf, manipConfs[confIdx - 1]))
confDistSq = 0
for j in range(len(qdiff)):
configDistSq += pow(qdiff[j], 2)
eConfDist = pow(configDistSq, 0.5)
# print "euclidean configuration distance:"
# print eConfDist
if (eConfDist > configurationJumpThreshold):
noConfigJump = False
return [False, '']
# If you made it here,
# it means no configuration jump, no collision, and
# the center of mass was close enough to the center
# of robot's feet for all reachability spheres.
return [True, activeDOFStartConfig]
else:
# start() failed
return [False, '']
h.append(misc.DrawAxes(env,T0_LF,0.1))
h.append(misc.DrawAxes(env,T0_RF,0.1))
goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 '+footlinknames
#+' movecog '+cogtargStr
+' nummanips 2 maniptm 2 '+trans_to_str(T0_LF)
+' maniptm 3 '+trans_to_str(T0_RF)
)
print "goalik"
print goalik
if(goalik != ''):
robots[idx].SetDOFValues(str2num(goalik),range(35))
print "inBalance"
print probs[0].SendCommand('CheckSupport supportlinks 2 '+footlinknames)
print "let's calculate the COM here..."
# https://en.wikipedia.org/wiki/Center_of_mass
# R*M = SUM(r_i*m_i)
#
# R = SUM(r_i*m_i)/M
M = 0
rm = 0
for l in robots[idx].GetLinks():
m = l.GetMass()
M += m
# print arg2
# print arg3
# print arg4
if( self.StopAtKeyStrokes ):
print "Press Enter to find a goalIK"
sys.stdin.readline()
self.crankid.SetDOFValues([crank_rot],[crankjointind])
goalik = cbirrtHubo.solve('DoGeneralIK exec supportlinks 2 '+footlinknames+' movecog '+arg1+' nummanips 3 maniptm 0 '+arg2+' maniptm 1 '+arg3+' maniptm 2 '+arg4)
# print "goalIK"
# print goalik
self.robotid.SetActiveDOFValues(str2num(goalik))
self.crankid.SetDOFValues([crank_rot],[crankjointind])
if( self.StopAtKeyStrokes ):
print "Press Enter to go to startik"
sys.stdin.readline()
# Get a trajectory from goalik to grasp configuration
goaljoints = deepcopy(goalik)
for i in range(TSRChainMimicDOF):
goaljoints += ' 0'
goaljoints = str2num(goaljoints)
self.robotid.SetActiveDOFValues(startik)
time.sleep(0.5)
# Uncomment to see T0_RH2
handles.append(misc.DrawAxes(env,matrix(T0_RH2),1))
print "Press Enter to find a goalIK"
sys.stdin.readline()
crankid.SetDOFValues([crank_rot],[crankjointind])
arg1 = str(cogtarg).strip("[]").replace(', ',' ')
arg2 = trans_to_str(T0_LH2)
arg3 = trans_to_str(T0_RH2)
goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 '+footlinknames+' movecog '+arg1+' nummanips 2 maniptm 0 '+arg2+' maniptm 1 '+arg3)
robotid.SetActiveDOFValues(str2num(goalik))
crankid.SetDOFValues([crank_rot],[crankjointind])
crankid.SetDOFValues([0],[crankjointind])
robotid.SetActiveDOFValues(initconfig)
robotid.SetDOFValues(rhandopenvals,rhanddofs)
robotid.SetDOFValues(lhandopenvals,lhanddofs)
print "Press Enter to plan initconfig --> startik"
sys.stdin.readline()
# Get a trajectory from initial configuration to grasp configuration
with robotid:
try:
answer = probs[0].SendCommand('RunCBiRRT psample 0.2 supportlinks 2 '+footlinknames+' smoothingitrs '+str(normalsmoothingitrs)+' '+TSRChainStringGrasping)
os.remove("movetraj" + str(i) + ".txt")
except OSError, e:
# print e
pass
#######################################
## CREATE TRAJ 0: INIT-->APPROACH 1 ##
#######################################
self.robot.SetActiveDOFValues(self.initconfig)
#############################################
## CREATE TRAJ 2: APPROACH 2 --> STARTIK ##
#############################################
goaljoints = str2num(self.startik)
if not self.plan_and_save_trajectory(goaljoints, "movetraj2.txt", False, False):
return 0
#######################################
## CREATE TRAJ 3: STARTIK --> GOALIK ##
#######################################
goaljoints = str2num(self.goalik)
goaljoints = concatenate((goaljoints, array([0])), axis=1)
T0_w0L = MakeTransform(matrix(rodrigues([self.wristPitch, 0, 0])), transpose(matrix([0, 0, 0])))
T0_w0R = MakeTransform(
rodrigues([0, self.wristPitch, 0]), matrix([[self.jointtm[9]], [self.jointtm[10]], [self.jointtm[11]]])
)
TSRChainString = SerializeTSRChain(0,0,1,1,TSRstring1,'crank',matrix([crankjointind]))+' '+SerializeTSRChain(0,0,1,1,TSRstring2,'NULL',[])
print TSRChainString
# Clean the trajectory files so that we don't execute an old trajectory
for i in range(2):
try:
print "Removing movetraj"+str(i)+".txt"
os.remove("movetraj"+str(i)+".txt")
except OSError, e:
print e
# Set the current goal to RH1 and LH1
goaljoints = str2num(startik);
#print goaljoints
print "Press enter to continue 2.1.."
# sys.stdin.readline()
self.robot.SetActiveDOFValues(initconfig)
self.crankid.SetDOFValues([0],[crankjointind])
#print goaljoints
print "Press enter to continue 2.2..."
# sys.stdin.readline()
TSRString_G1 = SerializeTSR(4,'NULL',T0_RH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
TSRString_G2 = SerializeTSR(7,'NULL',T0_LH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
TSRChainString_G = SerializeTSRChain(0,0,1,1,TSRString_G1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRString_G2,'NULL',[])
# Note: It turns out the PR2 has 8 manipulators in OpenRAVE
#
# maniptm 6: leftarm_torso (this coordinate frame is in the middle of the gripper)
# maniptm 4: rightarm_torso (this coordinate frame is in the middle of the gripper)
#
#
startik = probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 6 '+arg1+' maniptm 4 '+arg2)
print "Got startik:"
print startik
print "------"
# Matlab version has the following line but in Python it doesn't seem to do anything,
# Mostly beause probs[0].SendCommand makes the manipulators move already.
robotid.SetActiveDOFValues(str2num(startik)) # Note: startik: T0_LH1 and T0_RH1
print "go to startik"
sys.stdin.readline()
# Note: GetDOFValues() returns all 39 joints' values, whereas GetActiveDOFValues returns only the active joints' values
# THIS IS THE END OF THE SECOND BLOCK IN MATLAB VERSION
# Left hand is evaluated first, so need to make right hand relative to crank
cranklinks = crankid.GetLinks();
# debug
# print cranklinks
# for cl in range(len(cranklinks)):
# print cranklinks[cl].GetName()
Trobot = array([-0.0024,1,0,-1,-0.0024,0,0,0,1,1.2996,-0.6217,0.000]).reshape(4,3).T
robot.SetTransform(Trobot)
robot.SetActiveDOFs(r_[0, 1, 2, 3, 4, 5, 6, 11, 12, 13, 14, 15, 16, 17])
robot.SetActiveDOFValues(initdofvals)
handdof = r_[(2*ones([1,3]))[0]];
robot.SetActiveDOFs([7, 8, 9])
robot.SetActiveDOFValues(handdof)
#set the active dof
robot.SetActiveDOFs(activedofs)
#get info about the fridge hinge
fridgejointind = 6
jointtm = str2num(probs_cbirrt.SendCommand('GetJointTransform name kitchen jointind %d'%fridgejointind))
#set up the grasp
T0_RH1 = MakeTransform(rodrigues([0,0,pi])*(mat([-1, 0, 0, 0, 0, -1, 0, -1, 0]).T).reshape(3,3), mat([1.4351, -0.21, 1.1641]).T)
startik = str2num(probs_cbirrt.SendCommand('DoGeneralIK exec nummanips 1 maniptm 0 %s'%SerializeTransform(T0_RH1)))
robot.SetActiveDOFValues(startik)
time.sleep(0.5) #let the simulator draw the scene
#define TSR chains
#place the first TSR's reference frame at the door's hinge with no rotation relative to world frame
T0_w0 = MakeTransform(rodrigues([0,0,pi])*(mat([1, 0, 0, 0, 1, 0, 0, 0, 1]).T).reshape(3,3),mat(jointtm[9:12]).T)
handrot0 = mat(rodrigues([-pi/2, 0, 0]))
handtrans1 = T0_object[0:3,3] + transoffset1;
handrot1 = mat(rodrigues([pi/2, 0, 0]))
Tik0 = MakeTransform(handrot0,handtrans0);
Tik1 = MakeTransform(handrot1,handtrans1);
#get the ik solutions for both arms for the box in the start pose
robot.SetActiveDOFs(arm0dofs)
startik0 = probs_cbirrt.SendCommand('DoGeneralIK exec nummanips 1 maniptm 0 %s'%SerializeTransform(Tik0))
robot.SetActiveDOFs(arm1dofs)
startik1 = probs_cbirrt.SendCommand('DoGeneralIK exec nummanips 1 maniptm 1 %s'%SerializeTransform(Tik1))
startik = '%s %s'%(startik0,startik1)
robot.SetActiveDOFs(activedofs)
robot.SetActiveDOFValues(str2num(startik))
#define the target transform of the goal
T0_object2 = MakeTransform(mat(eye(3)),mat([0.6923,0.00,1.3989]).T)
targobject.SetTransform(array(T0_object2[0:3][:,0:4]))
#define targets for both hands relative to box in goal pose
handtrans0 = T0_object2[0:3,3] + transoffset0;
handtrans1 = T0_object2[0:3,3] + transoffset1;
Tik0 = MakeTransform(handrot0,handtrans0);
Tik1 = MakeTransform(handrot1,handtrans1);
self.robotid.WaitForController(0)
self.robotid.GetController().Reset(0)
self.robotid.SetDOFValues(self.rhandclosevals, self.rhanddofs)
self.robotid.SetDOFValues(self.lhandclosevals, self.lhanddofs)
if (self.StopAtKeyStrokes):
print "Press Enter to plan startik --> goalik "
sys.stdin.readline()
# Get a trajectory from goalik to grasp configuration
goaljoints = deepcopy(goalik)
for i in range(TSRChainMimicDOF):
goaljoints += ' 0'
goaljoints = str2num(goaljoints)
try:
answer = probs[0].SendCommand(
'RunCBiRRT supportlinks 2 ' + footlinknames +
' smoothingitrs ' + str(fastsmoothingitrs) + ' jointgoals ' +
str(len(goaljoints)) + ' ' +
Serialize1DMatrix(matrix(goaljoints)) + ' ' + TSRChainString)
print "RunCBiRRT answer: ", str(answer)
except openrave_exception, e:
print "Cannot send command RunCBiRRT: "
print e
try:
os.rename("cmovetraj.txt", "movetraj1.txt")
except OSError, e:
