def handle_update_valve_pose(self,req):
wheel = req.valve
print "wheel pose.position"
print wheel.pose.position
r = req.radius
print "Radius"
print r
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheel.pose.orientation.w,wheel.pose.orientation.x,wheel.pose.orientation.y,wheel.pose.orientation.z]),matrix([wheel.pose.position.x,wheel.pose.position.y,wheel.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame
# Wheel's model is defined rotated, the following transform will make it look straight up
wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0])))
wheelStraightUp = dot(wheelStraightUp,MakeTransform(rodrigues([-0.35,0,0]),transpose(matrix([0,0,0])))) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset.
wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel) # Wheel's pose in world coordinates, adjusted for torso lift link transform
wheelAtCorrectLocation = dot(wheelAtCorrectLocation,wheelStraightUp) # Rotate the model so that it looks straight up
self.crankid.SetTransform(array(wheelAtCorrectLocation))
res = updateValvePosResponse()
cx = wheel.pose.position.x # current x value for the valve in torso_lift_link
cy = wheel.pose.position.y # current y value for the valve in torso_lift_link
print "cx"
print cx
print "cy"
print cy
d = [cx,cy] # delta
print "delta"
print d
self.moveArmsToInitPosition()
rot_res = self.rotate_base(d)
if( rot_res == 1):
appr_res = self.approach_base(d)
if(appr_res == 1):
# Now we know where the valve is located, let's run the test points
if(self.generate_points() == 1):
# If tests succeeded go on creating the trajectories
if(self.arm_planner() == 1):
# Finally finish by playing the trajectories and return the code
task_control_response = pr2_rave2task_control() # This function packs movetraj files in 3 packages and sends them over to task_control.
res.success_code = task_control_response.result # Just return whatever the task control is returning back to GUI
else:
res.success_code = 'arm_planner failed'
else:
res.success_code = 'generate_points failed'
else:
res.success_code = 'approach is not good enough. align again.'
elif(rot_res == 2):
res.success_code = 'rotation is not good enough. align again.'
print 'End of pipeline - result:'
print res.success_code
return res
def run():
normalsmoothingitrs = 150;
fastsmoothingitrs = 20;
env = Environment()
RaveSetDebugLevel(DebugLevel.Info) # set output level to debug
env.Load('../../models/driving_wheel.robot.xml')
robotid = env.ReadRobotURI('robots/pr2-beta-static.zae')
crankid = env.GetRobots()[0]
crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix([0.5,0,0.8128])))))
env.Add(robotid)
env.SetViewer('qtcoin')
probs_cbirrt = RaveCreateModule(env,'CBiRRT')
probs_crankmover = RaveCreateModule(env,'CBiRRT')
manips = robotid.GetManipulators()
crankmanip = crankid.GetManipulators()
try:
env.AddModule(probs_cbirrt,'pr2')
env.AddModule(probs_crankmover,'crank')
except openrave_exception, e:
print e
def handle_update_valve_pose(self,req):
wheel = req.valve
print "wheel pose.position"
print wheel.pose.position
r = req.radius
print "Radius"
print r
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheel.pose.orientation.w,wheel.pose.orientation.x,wheel.pose.orientation.y,wheel.pose.orientation.z]),matrix([wheel.pose.position.x,wheel.pose.position.y,wheel.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame
# Wheel's model is defined rotated, the following transform will make it look straight up
wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0])))
wheelStraightUp = dot(wheelStraightUp,MakeTransform(rodrigues([-0.35,0,0]),transpose(matrix([0,0,0])))) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset.
wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel) # Wheel's pose in world coordinates, adjusted for torso lift link transform
wheelAtCorrectLocation = dot(wheelAtCorrectLocation,wheelStraightUp) # Rotate the model so that it looks straight up
self.crankid.SetTransform(array(wheelAtCorrectLocation))
res = updateValvePosResponse()
res.success_code = "yeeha"
return res
def wheel_pose_callback(self, wheelPose):
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(
rotationMatrixFromQuat(
[
wheelPose.pose.orientation.w,
wheelPose.pose.orientation.x,
wheelPose.pose.orientation.y,
wheelPose.pose.orientation.z,
]
),
matrix([wheelPose.pose.position.x, wheelPose.pose.position.y, wheelPose.pose.position.z]),
) # Transform from torso_lift_link to racing wheel's frame
# Wheel's model is defined rotated, the following transform will make it look straight up
wheelStraightUp = MakeTransform(
matrix(dot(rodrigues([0, 0, -pi / 2]), rodrigues([pi / 2, 0, 0]))), transpose(matrix([0, 0, 0]))
)
wheelAtCorrectLocation = dot(
T0_tll, Ttll_wheel
) # Wheel's pose in world coordinates, adjusted for torso lift link transform
wheelAtCorrectLocation = dot(
wheelAtCorrectLocation, wheelStraightUp
) # Rotate the model so that it looks straight up
self.crankid.SetTransform(array(wheelAtCorrectLocation))
def openrave_only_set_wheel_pose(self):
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(
rotationMatrixFromQuat([-0.0434053950012, 0.205337584019, 0.0024010904599, 0.977725327015]),
matrix([0.561164438725, 0.00322353374213, 0.101688474417]),
) # Transform from torso_lift_link to racing wheel's frame
# Wheel's model is defined rotated, the following transform will make it look straight up
wheelStraightUp = MakeTransform(
matrix(dot(rodrigues([0, 0, -pi / 2]), rodrigues([pi / 2, 0, 0]))), transpose(matrix([0, 0, 0]))
)
wheelStraightUp = dot(
wheelStraightUp, MakeTransform(rodrigues([-0.4, 0, 0]), transpose(matrix([0, 0, 0])))
) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset.
wheelAtCorrectLocation = dot(
T0_tll, Ttll_wheel
) # Wheel's pose in world coordinates, adjusted for torso lift link transform
wheelAtCorrectLocation = dot(
wheelAtCorrectLocation, wheelStraightUp
) # Rotate the model so that it looks straight up
self.crankid.SetTransform(array(wheelAtCorrectLocation))
def run():
normalsmoothingitrs = 150;
fastsmoothingitrs = 20;
env = Environment()
RaveSetDebugLevel(DebugLevel.Info) # set output level to debug
env.Load('../../models/driving_wheel.robot.xml')
robotid = env.ReadRobotURI('robots/pr2-beta-static.zae')
crankid = env.GetRobots()[0]
# Move the wheel to the front of the robot, somewhere it can grasp and rotate
# Note that when mounted on our foldable table the height of the wheel is 0.8128
crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix([0.5,0,0.8128])))))
env.Add(robotid)
env.SetViewer('qtcoin')
# Note: RaveCreateProblem will be deprecated and it uses RaveCrateModule in it anyways so we just use RaveCreateModule instead
probs_cbirrt = RaveCreateModule(env,'CBiRRT')
probs_crankmover = RaveCreateModule(env,'CBiRRT')
manips = robotid.GetManipulators()
crankmanip = crankid.GetManipulators()
# for m in range(len(manips)):
# print manips[m].armjoints
# Note: The argument you pass in must be the same as (the robot name) defined in the .zae or .xml file
# Try - Catch is very useful to figure out what the problem is (if any) with cbirrt function calls
try:
env.AddModule(probs_cbirrt,'pr2')
env.AddModule(probs_crankmover,'crank')
except openrave_exception, e:
print e
def wheel_pose_callback(self,wheelPose):
if(self.notPlanning == True): # We don't want the wheel to move during trajectory planning
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheelPose.pose.orientation.w,wheelPose.pose.orientation.x,wheelPose.pose.orientation.y,wheelPose.pose.orientation.z]),matrix([wheelPose.pose.position.x,wheelPose.pose.position.y,wheelPose.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame
wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0])))
wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel)
wheelAtCorrectLocation = dot(wheelAtCorrectLocation,wheelStraightUp)
self.crankid.SetTransform(array(wheelAtCorrectLocation))
def callback(self,wheelPose):
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheelPose.pose.orientation.w,wheelPose.pose.orientation.x,wheelPose.pose.orientation.y,wheelPose.pose.orientation.z]),matrix([wheelPose.pose.position.x,wheelPose.pose.position.y,wheelPose.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame
#wheel_x = torso_lift_link[0][3]+data.data[0]
#wheel_y = torso_lift_link[1][3]+data.data[1]
#wheel_z = torso_lift_link[2][3]+data.data[2]
#w=[wheel_x,wheel_y,wheel_z]
wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0])))
wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel)
wheelAtCorrectLocation = dot(wheelAtCorrectLocation, wheelStraightUp)
self.crankid.SetTransform(array(wheelAtCorrectLocation))
def save_markers_to_file(self):
with open("./matlab/markers_tmp.csv", 'w') as m_file:
line_str = ""
# Construct frame centered at torso with orientation
# set by the pelvis frame, add rotation offset for matlab code
t_trans = deepcopy(self.t_pelvis)
t_trans[0:3, 3] = deepcopy(self.t_torso[0:3, 3])
t_trans = t_trans * \
MakeTransform(rodrigues([0, 0, pi]), matrix([0, 0, 0]))
# inv_pelvis = la.inv(self.t_pelvis)
# self.handles.append(misc.DrawAxes(self.env, inv_pelvis * t_trans, 2))
t_trans = la.inv(t_trans)
for marker in self.markers:
marker = array(array(t_trans).dot(append(marker, 1)))[0:3]
line_str += str(marker[0] * 1000) + ','
line_str += str(marker[1] * 1000) + ','
line_str += str(marker[2] * 1000) + ','
line_str = line_str.rstrip(',')
line_str += '\n'
m_file.write(line_str)
def get_rotate_cw(minRotAngle, maxRotAngle, delta1, delta2, r):
rotTrajsL = [] # rotational trajectories for the left hand
rotTrajsR = [] # rotational trajectories for the right hand
for angle in frange(minRotAngle, maxRotAngle, delta1):
currentTrajL = []
currentTrajR = []
xyrL = RotationalTrajDiscretizer.get_discrete_xyr(
r, pi, pi - angle, -0.01)
rawL = xyrL[0]
discreteL = xyrL[1]
#print "got discrete elements - L"
for element in discreteL:
#print element
adjustedY = -1.0 * (element[0] + r) + 0.0
adjustedZ = element[1]
adjustedR = (4.0 - element[2]) * (pi / 4.0)
currentTrajL.append(
array(
MakeTransform(
matrix(rodrigues([adjustedR, 0, 0])),
transpose(matrix([0.0, adjustedY, adjustedZ])))))
#print str(adjustedY), str(adjustedZ), str(adjustedR)
xyrR = RotationalTrajDiscretizer.get_discrete_xyr(
r, 0.0, -1.0 * angle, -0.01)
rawR = xyrR[0]
discreteR = xyrR[1]
# print "got discrete elements - R"
for element in discreteR:
# print element
adjustedY = r - element[0]
adjustedZ = element[1]
adjustedR = (-1.0 * element[2] + 0.0) * (pi / 4.0)
# print str(adjustedY), str(adjustedZ), str(adjustedR)
currentTrajR.append(
array(
MakeTransform(
matrix(rodrigues([adjustedR, 0, 0])),
transpose(matrix([0.0, adjustedY, adjustedZ])))))
rotTrajsL.append(currentTrajL)
rotTrajsR.append(currentTrajR)
return [rotTrajsL, rotTrajsR]
def callback(self,data):
#self.crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix([0.5,0,0.8128])))))
torso_lift_link = self.robot.GetLinks()[16].GetTransform()
wheel_x = torso_lift_link[0][3]+data.data[0]
wheel_y = torso_lift_link[1][3]+data.data[1]
wheel_z = torso_lift_link[2][3]+data.data[2]
w=[wheel_x,wheel_y,wheel_z]
self.crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix(w)))))
print "4"
def get_push(minTrajLength,maxTrajLength, delta1, delta2):
pushTrajsL = []
for length in frange(minTrajLength,maxTrajLength,delta1):
currentTraj = []
for l in frange(0,length,delta2):
currentTraj.append(array(MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([l, 0.0, 0.0])))))
pushTrajsL.append(currentTraj)
pushTrajsR = deepcopy(pushTrajsL)
return [pushTrajsL, pushTrajsR]
def get_lift(minTrajLength, maxTrajLength, delta1, delta2):
liftTrajsL = []
for length in frange( minTrajLength, maxTrajLength, delta1):
currentTraj = []
for l in frange(0, length, delta2):
currentTraj.append(array(MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, 0.0, l])))))
liftTrajsL.append(currentTraj)
liftTrajsR = deepcopy(liftTrajsL)
return [liftTrajsL, liftTrajsR]
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
def run(candidates, leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm, T0_LH, T0_RH, relBaseConstraint):
mySamples = []
# 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_starts = []
T0_starts.append(array(T0_LH))
T0_starts.append(array(T0_RH))
whereToFace = MakeTransform(rodrigues([0,0,0]),transpose(matrix([0,0,0])))
# 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, activeDOFStartConfigStr] = play(T0_starts, whereToFace, relBaseConstraint, candidates, numRobots, numManips, c ,myRmaps,[robot], h, env, 0.0, True, ' leftFootBase rightFootBase ')
h.pop() # delete the robot base axis we added last
# We went through all our constraints. Is the candidate valid?
if(allGood):
collisionFreeSolutions[0].append(c)
collisionFreeSolutions[1].append(activeDOFStartConfigStr)
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[0]))," collision-free solutions in ",str(howMany)," candidates."
if (len(collisionFreeSolutions[0]) > 0) :
print "FOUND COLLISION FREE SOLUTIONS!!! WHOOHOO!!!! WILL PLAY RESULTS!!!"
# sys.stdin.readline()
# Have we found at least 1 collision free path?
for colFreeSolIdx, solIdx in enumerate(collisionFreeSolutions[0]):
mySamples.append([candidates[0][solIdx],candidates[1][solIdx],collisionFreeSolutions[1][colFreeSolIdx]])
return mySamples
def get_lift(minTrajLength, maxTrajLength, delta1, delta2):
liftTrajsL = []
for length in frange(minTrajLength, maxTrajLength, delta1):
currentTraj = []
for l in frange(0, length, delta2):
currentTraj.append(
array(
MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, l])))))
liftTrajsL.append(currentTraj)
liftTrajsR = deepcopy(liftTrajsL)
return [liftTrajsL, liftTrajsR]
def get_rotate_cw( minRotAngle, maxRotAngle, delta1, delta2, r):
rotTrajsL = [] # rotational trajectories for the left hand
rotTrajsR = [] # rotational trajectories for the right hand
for angle in frange( minRotAngle, maxRotAngle, delta1):
currentTrajL = []
currentTrajR = []
xyrL = RotationalTrajDiscretizer.get_discrete_xyr(r, pi, pi-angle, -0.01)
rawL = xyrL[0]
discreteL = xyrL[1]
#print "got discrete elements - L"
for element in discreteL:
#print element
adjustedY = -1.0*(element[0]+r)+0.0
adjustedZ = element[1]
adjustedR = (4.0 - element[2])*(pi/4.0)
currentTrajL.append(array(MakeTransform(matrix(rodrigues([adjustedR, 0, 0])),transpose(matrix([0.0, adjustedY, adjustedZ])))))
#print str(adjustedY), str(adjustedZ), str(adjustedR)
xyrR = RotationalTrajDiscretizer.get_discrete_xyr(r, 0.0, -1.0*angle, -0.01)
rawR = xyrR[0]
discreteR = xyrR[1]
# print "got discrete elements - R"
for element in discreteR:
# print element
adjustedY = r-element[0]
adjustedZ = element[1]
adjustedR = (-1.0*element[2]+0.0)*(pi/4.0)
# print str(adjustedY), str(adjustedZ), str(adjustedR)
currentTrajR.append(array(MakeTransform(matrix(rodrigues([adjustedR, 0, 0])),transpose(matrix([0.0, adjustedY, adjustedZ])))))
rotTrajsL.append(currentTrajL)
rotTrajsR.append(currentTrajR)
return [rotTrajsL, rotTrajsR]
def get_push(minTrajLength, maxTrajLength, delta1, delta2):
pushTrajsL = []
for length in frange(minTrajLength, maxTrajLength, delta1):
currentTraj = []
for l in frange(0, length, delta2):
currentTraj.append(
array(
MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([l, 0.0, 0.0])))))
pushTrajsL.append(currentTraj)
pushTrajsR = deepcopy(pushTrajsL)
return [pushTrajsL, pushTrajsR]
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
def get_robot_com(myRobot):
# print "let's calculate the COM here..."
#
# This is how the center of mass is calculated
#
# 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 myRobot.GetLinks():
# mass of this link
m = l.GetMass()
# keep the total mass of the robot
M += m
# l.GetTransform() does not return the center of mass of the link
r = dot(
l.GetTransform(),
array(
MakeTransform(rodrigues([0, 0, 0]),
transpose(matrix(l.GetCOMOffset())))))
# multiplication by element
rm += multiply(r, m)
# print "total mass:"
# print M
# division by element
R = divide(rm, M)
# print "T0_COM for "+myRobot.GetName()
# print R.round(4)
# Draw the center of mass if you want
# h.append(misc.DrawAxes(myEnv,array(R.round(4)),0.1))
return R
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm):
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
myPatterns.append(myPattern)
# 3. Create a search pattern from the trajectory for the second manipulator
myPattern = SearchPattern(rightTraj)
myPattern.T0_p = T0_p
myPatterns.append(myPattern)
candidates = None
print "Ready to look for candidates... ",str(datetime.now())
candidates = look_for_candidates(pairs, rm, myPatterns)
return candidates
def find_neighbors(self):
print "Finding neighbors..."
# For all spheres in the map do:
for index, sphere in enumerate(self.map):
sphere.neighbors = []
# For each possible neighbor do:
for Tsphere_neighbor in self.Tsphere_neighbors:
# Erase rotation (we don't care), keep transformation.
Tbase_sphere = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([sphere.T[0][0,3],sphere.T[0][1,3],sphere.T[0][2,3]])))
# Convert the transform into robot's base coordinates
Tbase_neighbor = dot(Tbase_sphere,Tsphere_neighbor)
# Convert the coordinates into a string
key = str(round(Tbase_neighbor[0,3],2)),",",str(round(Tbase_neighbor[1,3],2)),",",str(round(Tbase_neighbor[2,3],2))
# If there is a reachability sphere at these coordinates
if( key in self.indices ):
# Look-up the index of the neighbor and keep it
sphere.neighbors.append(self.indices[key])
print "Finding neighbors, done."
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs,
rm):
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
myPatterns.append(myPattern)
# 3. Create a search pattern from the trajectory for the second manipulator
myPattern = SearchPattern(rightTraj)
myPattern.T0_p = T0_p
myPatterns.append(myPattern)
candidates = None
print "Ready to look for candidates... ", str(datetime.now())
candidates = look_for_candidates(pairs, rm, myPatterns)
return candidates
def get_robot_com(myRobot):
# print "let's calculate the COM here..."
#
# This is how the center of mass is calculated
#
# 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 myRobot.GetLinks():
# mass of this link
m = l.GetMass()
# keep the total mass of the robot
M += m
# l.GetTransform() does not return the center of mass of the link
r = dot(l.GetTransform(),array(MakeTransform(rodrigues([0,0,0]),transpose(matrix(l.GetCOMOffset())))))
# multiplication by element
rm += multiply(r,m)
# print "total mass:"
# print M
# division by element
R = divide(rm,M)
# print "T0_COM for "+myRobot.GetName()
# print R.round(4)
# Draw the center of mass if you want
# h.append(misc.DrawAxes(myEnv,array(R.round(4)),0.1))
return R
def get_markers_in_pelvis_frame(self, markers, t_pelvis):
# Construct frame centered at torso with orientation
# set by the pelvis frame, add rotation offset for matlab code
trunk_center = (markers[0] + markers[1]) / 2
self.t_trans = deepcopy(t_pelvis)
self.t_trans[0:3, 3] = transpose(matrix(trunk_center))
self.t_trans = self.t_trans * \
MakeTransform(rodrigues([0, 0, pi]), matrix([0, 0, 0]))
inv_t_trans = la.inv(self.t_trans)
points_3d = len(markers) * [array([0., 0., 0.])]
for i, p in enumerate(markers):
points_3d[i] = array(
array(inv_t_trans).dot(array(append(p, 1.0))))[0:3]
# points_3d[i] *= 1000
return points_3d
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs,
rm):
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()
candidates = None
print "Ready to look for candidates... ", str(datetime.now())
candidates = look_for_candidates(pairs, rm, myPatterns)
return candidates
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm):
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()
candidates = None
print "Ready to look for candidates... ",str(datetime.now())
candidates = look_for_candidates(pairs, rm, myPatterns)
return candidates
# The following two matrices define no rotation. Only translation
# T0_LH1=matrix([[1,0,0,0.5],[0,1,0,0.3],[0,0,1,1.2],[0,0,0,1]])
# T0_RH1=matrix([[1,0,0,0.5],[0,1,0,-0.3],[0,0,1,1.1],[0,0,0,1]])
# The following two matrices define translation and rotation of 90 degrees around WORLD's x-axis and the new coordinate frame's Z-axis
#
# For rotation around X, left hand should be rotated in the positive direction, whereas right hand should be rotated in the negative
# For rotation around Y both hands should be rotated in the negative direction
# KEEP THESE JUST IN CASE UNTIL YOU DEBUG
#T0_LH1 = MakeTransform(array(dot(rodrigues([pi/2,0,0]),rodrigues([0,0,pi/2]))),array([[0.51],[0.11],[0.8]]))
#T0_RH1 = MakeTransform(array(dot(rodrigues([-pi/2,0,0]),rodrigues([0,0,pi/2]))),array([[0.51],[-0.11],[0.8]]))
# 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]))))
# Translate the new coordinate frame -0.11 meters on its Z-axis and assign it to Left Hand
Left_Hand_Point_In_Wheel_Coordinate_Frame = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0,-0.11]))))
T0_LH1 = Left_Hand_Point_In_Wheel_Coordinate_Frame
# 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]))))
def handle_update_valve_pose(self, req):
# This is the main cycle that is triggered by RViz when the user clicks on Send2Robot
# Get the valve pose
wheel = req.valve
print "GUI sends the following:"
print wheel
# Get the valve radius
r = req.radius
T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame
Ttll_wheel = MakeTransform(
rotationMatrixFromQuat(
[wheel.pose.orientation.w, wheel.pose.orientation.x, wheel.pose.orientation.y, wheel.pose.orientation.z]
),
matrix([wheel.pose.position.x, wheel.pose.position.y, wheel.pose.position.z]),
) # Transform from torso_lift_link to racing wheel's frame
# Wheel's model is defined rotated, the following transform will make it look straight up
wheelStraightUp = MakeTransform(
matrix(dot(rodrigues([0, 0, -pi / 2]), rodrigues([pi / 2, 0, 0]))), transpose(matrix([0, 0, 0]))
)
wheelStraightUp = dot(
wheelStraightUp, MakeTransform(rodrigues([-0.4, 0, 0]), transpose(matrix([0, 0, 0])))
) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset.
wheelAtCorrectLocation = dot(
T0_tll, Ttll_wheel
) # Wheel's pose in world coordinates, adjusted for torso lift link transform
wheelAtCorrectLocation = dot(
wheelAtCorrectLocation, wheelStraightUp
) # Rotate the model so that it looks straight up
self.crankid.SetTransform(array(wheelAtCorrectLocation))
res = updateValvePosResponse()
cx = wheel.pose.position.x # current x value for the valve in torso_lift_link
cy = wheel.pose.position.y # current y value for the valve in torso_lift_link
d = [cx, cy] # delta
self.moveArmsToInitPosition()
self.planning_lock.acquire()
# rot_res = self.rotate_base(d)
cbirrt_result = -1
skip = -1
user_note = ""
if self.generate_points() == 1:
# print"generate_points succeeded"
if self.arm_planner() == 1:
cbirrt_result = 1
# res.success_code = "not using task_control."
# print "skip?"
# ans = sys.stdin.readline()
# if(ans[0] == 'y'):
# skip = 1
# res.success_code = "skipped"
# else:
# skip = 0
# # print"arm planner succeeded"
# # Finally finish by playing the trajectories and return the code
task_control_response = pr2_rave2task_control(
req.id
) # This function packs movetraj files in 3 packages and sends them over to task_control.
res.success_code = (
task_control_response.result
) # Just return whatever the task control is returning back to GUI
else:
cbirrt_result = 0
res.success_code = "arm planner failed"
else:
res.success_code = "generate points failed"
self.planning_lock.release()
if self.save_log:
print "Iteration #" + str(self.num_iteration)
# print "Enter your notes for this iteration:"
# user_note = sys.stdin.readline()
ts = str(datetime.now())[11:19]
data = [
self.num_iteration,
cbirrt_result,
req.id,
wheel.pose.position.x,
wheel.pose.position.y,
wheel.pose.position.z,
wheel.pose.orientation.x,
wheel.pose.orientation.y,
wheel.pose.orientation.z,
wheel.pose.orientation.w,
req.rms_err,
req.angle_err,
res.success_code.replace(",", "|"),
skip,
ts,
user_note[0 : len(user_note) - 1], # leave the \n in the end out
]
self.my_logger.save(data)
self.num_iteration = self.num_iteration + 1
print "End of pipeline - result:"
print res.success_code
return res
## 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]]])
)
Tw0_eL = dot(linalg.inv(self.T0_crankcrank), self.T0_LH1_ENTRY)
Tw0_eR = dot(linalg.inv(T0_w0R), self.T0_RH1_ENTRY)
Bw0L = matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
Bw0R = matrix([0, 0, 0, 0, 0, 0, -pi, pi, 0, 0, 0, 0])
TSRString_G_R = SerializeTSR(5, "NULL", T0_w0R, Tw0_eR, Bw0R) # TSR for the right gripper
TSRString_G_L = SerializeTSR(7, "crank crank", T0_w0L, Tw0_eL, Bw0L) # TSR for the left gripper
TSRChainString_StoG = (
SerializeTSRChain(0, 0, 1, 1, TSRString_G_R, "crank", matrix([self.crankjointind]))
+ " "
# print "traj call answer: ",str(answer) # except openrave_exception, e: # print e # Get the current configuration of the robot # and assign it to startik (start of the wheel # rotation path). startik = self.robotid.GetActiveDOFValues() # Left Hand's index is less than the right hand. # Hence it is evaluated first by the CBiRRT Module. # That's why We need to define the right hand's # transform relative to the wheel (ask Dmitry Berenson # about this for more information). temp1 = MakeTransform(rodrigues([-pi/2,0,0]),transpose(matrix([0,0,0]))) temp2 = MakeTransform(rodrigues([0,0,-pi/2]),transpose(matrix([0,0,0]))) # 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 = dot(dot(CTee,temp1),temp2) # 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
def play(relBaseConstraint,candidates,numRobots,c,myRmaps,robots,h,env):
# constraints to check
relBaseConstOK = True
collisionConstOK = True
noConfigJump = True
if(start(candidates,numRobots,c,myRmaps,robots,h,env)):
# Get their relative Transformation matrix
T0_base = []
for myManipulatorIndex in range(len(robots)):
T0_base.append(robots[myManipulatorIndex].GetManipulators()[0].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 robot to this path element
for myRobotIndex in range(numRobots):
currentSphereIndex = candidates[myRobotIndex][c][pElementIndex].sIdx
currentTransformIndex = candidates[myRobotIndex][c][pElementIndex].tIdx
myRmaps[myRobotIndex].go_to(currentSphereIndex,currentTransformIndex)
pathConfigs[myRobotIndex].append(robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[0].GetArmIndices()))
# DEBUG SECTION FOR SENSING CONFIGURATION JUMP
currentConfig[myRobotIndex] = robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[0].GetArmIndices())
# print "For robot ",str(myRobotIndex)," ||qA-qB||:"
# print "path element ",str(pElementIndex)
if(prevConfig[myRobotIndex] != []):
# print "previous config: "
# print prevConfig[myRobotIndex]
# print "current config: "
# print currentConfig[myRobotIndex]
qdiff = absolute(subtract(currentConfig[myRobotIndex],prevConfig[myRobotIndex]))
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[myRobotIndex] = deepcopy(currentConfig[myRobotIndex])
# Check collision with self and with the environment
if(env.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()):
collisionConstOK = False
return False
# If you didn't break yet, wait before the next path element for visualization
time.sleep(0.1)
# If you made it here,
# it means no configuration jump, and no collision
return True
else:
# start() failed
return False
from TransformMatrix import *
from str2num import *
from TSR import *
if __name__=='__main__':
t="wheel"
InitOpenRAVELogging()
RaveSetDebugLevel(DebugLevel.Verbose)
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
robot = env.ReadRobotXMLFile('../../../openHubo/huboplus/rlhuboplus.robot.xml')
env.AddRobot(robot)
if(t=="wheel"):
target = env.ReadKinBodyXMLFile('../../models/driving_wheel.kinbody.xml')
T1 = MakeTransform(rodrigues([0,0,pi/2]),transpose(matrix([0,0,0])))
T2 = MakeTransform(rodrigues([pi/2,0,0]),transpose(matrix([0,0,0])))
T3 = dot(T1,T2)
T4 = MakeTransform(eye(3),transpose(matrix([0, 0.8, 1.0])))
T_wheel = dot(T3,T4)
target.SetTransform(array(T_wheel))
elif(t=="mug"):
target = env.ReadKinBodyXMLFile('../../models/mug1.kinbody.xml')
robot.SetDOFValues([-0.7,-0.7],[19,20])
manip = robot.SetActiveManipulator('leftArm')
env.AddKinBody(target)
sys.stdin.readline()
gmodel = grasping.GraspingModel(robot=robot,target=target)
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, '']
self.bConstrainToChain, numTSRs, allTSRstring, self.mimicbodyname)
if size(self.mimicbodyjoints) != 0:
outstring += ' %d %s ' % (size(self.mimicbodyjoints),
Serialize1DIntegerMatrix(
self.mimicbodyjoints))
return outstring
def SetFirstT0_w(self, T0_w_in):
self.TSRs[0].T0_w = copy.deepcopy(T0_w_in)
if __name__ == '__main__':
juiceTSR = TSR()
juiceTSR.Tw_e = MakeTransform(rodrigues([pi / 2, 0, 0]),
mat([0, 0.22, 0.1]).T)
juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi])
juiceTSR.manipindex = 0
juiceTSRChain1 = TSRChain(1, 0)
juiceTSRChain1.insertTSR(juiceTSR)
juiceTSR.Tw_e = MakeTransform(
rodrigues([0, pi, 0]) * rodrigues([pi / 2, 0, 0]),
mat([0, 0.22, 0.1]).T)
juiceTSRChain2 = TSRChain(1, 0)
juiceTSRChain2.insertTSR(juiceTSR)
print juiceTSRChain1.Serialize()
print juiceTSRChain2.Serialize()
from TSR import *
if __name__ == '__main__':
t = "wheel"
InitOpenRAVELogging()
RaveSetDebugLevel(DebugLevel.Verbose)
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
robot = env.ReadRobotXMLFile(
'../../../openHubo/huboplus/rlhuboplus.robot.xml')
env.AddRobot(robot)
if (t == "wheel"):
target = env.ReadKinBodyXMLFile(
'../../models/driving_wheel.kinbody.xml')
T1 = MakeTransform(rodrigues([0, 0, pi / 2]),
transpose(matrix([0, 0, 0])))
T2 = MakeTransform(rodrigues([pi / 2, 0, 0]),
transpose(matrix([0, 0, 0])))
T3 = dot(T1, T2)
T4 = MakeTransform(eye(3), transpose(matrix([0, 0.8, 1.0])))
T_wheel = dot(T3, T4)
target.SetTransform(array(T_wheel))
elif (t == "mug"):
target = env.ReadKinBodyXMLFile('../../models/mug1.kinbody.xml')
robot.SetDOFValues([-0.7, -0.7], [19, 20])
manip = robot.SetActiveManipulator('leftArm')
env.AddKinBody(target)
sys.stdin.readline()
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)
if chainlength == 1:
#get the TSR's offset frame in w0 coordinates
arm1dofs = [11, 12, 13, 14, 15, 16, 17]
activedofs = arm0dofs + arm1dofs
initdofvals = r_[3.68, -1.9, -0.0000, 2.2022, -0.0000, 0.0000, 0.0000, 2.6, -1.9, -0.0000, 2.2022, -0.0001, 0.0000, 0.0000]
robot.SetActiveDOFs(activedofs)
robot.SetActiveDOFValues(initdofvals)
# orEnv.LockPhysics(False)
time.sleep(0.5) #let the simulator draw the scene
# orEnv.LockPhysics(True)
#define targets for both hands relative to box in start pose
transoffset0 = mat([0.0, -0.285, 0]).T
transoffset1 = mat([0.0, 0.305, 0]).T
handtrans0 = T0_object[0:3,3] + transoffset0;
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))
robot.SetActiveDOFValues(handdof)
time.sleep(0.5) #let the simulator draw the scene
#set the active dof
robot.SetActiveDOFs(activedofs)
#let's define two TSR chains for this task, they differ only in the rotation of the hand
#first TSR chain
#place the first TSR's reference frame at the object's frame relative to world frame
T0_w = T0_object
#get the TSR's offset frame in w coordinates
Tw_e1 = MakeTransform(rodrigues([pi / 2, 0, 0]), mat([0, 0.20, 0.1]).T)
#define bounds to only allow rotation of the hand about z axis and a small deviation in translation along the z axis
Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi])
TSRstring1 = SerializeTSR(0, 'NULL', T0_w, Tw_e1, Bw)
TSRChainString1 = SerializeTSRChain(0, 1, 0, 1, TSRstring1, 'NULL', [])
#now define the second TSR chain
#it is the same as the first TSR Chain except Tw_e is different (the hand is rotated by 180 degrees about its z axis)
Tw_e2 = MakeTransform(
rodrigues([0, pi, 0]) * rodrigues([pi / 2, 0, 0]),
mat([0, 0.20, 0.1]).T)
TSRstring2 = SerializeTSR(0, 'NULL', T0_w, Tw_e2, Bw)
TSRChainString2 = SerializeTSRChain(0, 1, 0, 1, TSRstring2, 'NULL', [])
rlhuboplus,
rlhuboplus.GetManipulators()[0])
rlhuboplusRightRm = ReachabilityMap("./rlhuboplus_rightArm_ik_solver",
rlhuboplus,
rlhuboplus.GetManipulators()[1])
print "loading"
rlhuboplusLeftRm.load("rlhuboplus_left_m12")
rlhuboplusRightRm.load("rlhuboplus_right_m12")
env.SetViewer('qtcoin')
h = misc.DrawAxes(
env,
array(
MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))), 1.0)
rlhuboplusLeftRm.map[1025].show(env, array((0, 0, 1, 0.5)))
rlhuboplusLeftRm.map[1026].show(env, array((0, 0, 1, 0.5)))
rlhuboplusLeftRm.map[1055].show(env, array((0, 0, 1, 0.5)))
rlhuboplusLeftRm.map[1056].show(env, array((0, 0, 1, 0.5)))
rlhuboplusLeftRm.map[1227].show(env, array((0, 0, 1, 0.5)))
rlhuboplusLeftRm.map[1228].show(env, array((0, 0, 1, 0.5)))
rlhuboplusLeftRm.hide()
rlhuboplusRightRm.map[1071].show(env, array((1, 0, 0, 0.5)))
rlhuboplusRightRm.map[1070].show(env, array((1, 0, 0, 0.5)))
rlhuboplusRightRm.map[1101].show(env, array((1, 0, 0, 0.5)))
rlhuboplusRightRm.map[1100].show(env, array((1, 0, 0, 0.5)))
rlhuboplusRightRm.map[1257].show(env, array((1, 0, 0, 0.5)))
def get_pairs(TLH_RH, env, robot, myRmaps):
# h.append(misc.DrawAxes(env,array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))),1.0))
gR = 0.0 # ground color
gG = 0.0 # ground color
gB = 0.0 # ground color
ge = 5.0 # ground extension
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((ge, 0, 0), (0, ge, 0), (ge, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(
((-ge, 0, 0), (0, -ge, 0), (-ge, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((-ge, 0, 0), (0, ge, 0), (-ge, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((ge, 0, 0), (0, -ge, 0), (ge, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
# Define robot base constraint(s)
# a) Bounds <type 'list'>
# xyz in meters, rpy in radians
# [1.0, 1.0, 1.0, 0.0, 0.0, 0.0] would mean, we allow robot bases
# to be 1 meter apart in each direction but we want them to have the
# same rotation
# Relative Base Constraints between the two robots
#relBaseConstraint = MakeTransform() # This is type (b)
relBaseConstraint = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))
# Base Constraints of the first (master) robot in the world coordinate frame
# Same as relative base constraints. First 3 elements are XYZ bounds and the last three elements determine the desired rotation matrix of the base of the master robot in world coords.
masterBaseConstraint = [1.0, 1.0, 0.0, 0.0, 0.0, 0.0]
# b) Exact transform <type 'numpy.ndarray'>
# relBaseConstraint = dot(something, some_other_thing)
# Try to find a valid candidate that satisfies
# all the constraints we have (base location, collision, and configuration-jump)
print "Ready to find sisters... ", str(datetime.now())
resp = find_sister_pairs(myRmaps, [relBaseConstraint], [TLH_RH], env)
# resp can be None or [pairs, rm, p]
if resp == None:
return resp
else:
return [resp[0], resp[1], [relBaseConstraint]]
if __name__ == '__main__':
env = Environment()
env.SetViewer('qtcoin')
# Add huboplus
robot = env.ReadRobotURI('../../../openHubo/huboplus/huboplus.robot.xml')
env.Add(robot)
lowerLimits, upperLimits = robot.GetDOFLimits()
# Let's keep the rotation of the robot around it's Z-Axis in a variable...
rotz = []
rotz.append(pi / 3)
# Define the transform and apply the transform
shift_robot0 = MakeTransform(matrix(rodrigues([0, 0, rotz[0]])),
transpose(matrix([-2.5, 0.0, 0.95])))
robot.SetTransform(array(shift_robot0))
probs_cbirrt = RaveCreateModule(env, 'CBiRRT')
try:
env.AddModule(probs_cbirrt, robot.GetName(
)) # this string should match to <Robot name="" > in robot.xml
except openrave_exception, e:
print e
print "Getting Loaded Problems"
probs = env.GetLoadedProblems()
# Load the reachability map for left arm
occupancy = {}
reachability_dict = {}
env = Environment()
RaveSetDebugLevel(1)
robot = env.ReadRobotURI(
'../../../openHubo/drchubo/drchubo-urdf/robots/drchubo.robot.xml')
env.Add(robot)
manips = robot.GetManipulators()
drchubo_rightFoot = manips[3]
rf_eet = drchubo_rightFoot.GetEndEffectorTransform(
) # right foot end effector transform
shift_drchubo_to_its_right_foot = MakeTransform(
matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.5, -1 * rf_eet[1][3], -1 * rf_eet[2][3]])))
robot.SetTransform(array(shift_drchubo_to_its_right_foot))
# box 0
mybox0 = RaveCreateKinBody(env, '')
mybox0.SetName('box_0')
mybox0.InitFromBoxes(numpy.array([[0.8, 0.45, 0.74, 0.025, 0.025, 0.3]]),
True) # set geometry as one box of extents 0.1, 0.2, 0.3
env.Add(mybox0, True)
# box 1
mybox1 = RaveCreateKinBody(env, '')
mybox1.SetName('box_1')
mybox1.InitFromBoxes(numpy.array([[0.7, -0.27, 1.3, 0.05, 0.05, 0.5]]),
True) # set geometry as one box of extents 0.1, 0.2, 0.3
## Constraint Based Manipulation ##
from rodrigues import *
from TransformMatrix import *
from str2num import *
from TSR import *
# robot-placement common functions
from roplace_common import *
# This changes the height and the pitch angle of the wheel
version = 1
env = Environment()
env.SetViewer('qtcoin')
T0_p = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))
# 1. Create a trajectory for the tool center point to follow
# Left Hand
traj0 = []
Tstart0 = array(
MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0]))))
traj0.append(Tstart0)
# traj0.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.05])))))
#traj0.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.05])))))
#traj0.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.1])))))
def get_pairs(pitch, height, dist, env, robot, myRmaps):
h.append(
misc.DrawAxes(
env,
array(
MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))), 1.0))
gR = 0.6 # ground color
gG = 0.6 # ground color
gB = 0.6 # ground color
ge = 5.0 # ground extension
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((ge, 0, 0), (0, ge, 0), (ge, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(
((-ge, 0, 0), (0, -ge, 0), (-ge, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((-ge, 0, 0), (0, ge, 0), (-ge, ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
h.append(
env.drawtrimesh(points=array(((ge, 0, 0), (0, -ge, 0), (ge, -ge, 0))),
indices=None,
colors=array(
((gR, gG, gB), (gR, gG, gB), (gR, gG, gB)))))
Tee = []
manips = robot.GetManipulators()
for i in range(len(manips)):
# Returns End Effector Transform in World Coordinates
Tlink = manips[i].GetEndEffectorTransform()
Tee.append(Tlink)
# Where do we want the end effectors (hands) to start from in world coordinates?
T0_LIFT = MakeTransform(matrix(rodrigues([0, pitch, 0])),
transpose(matrix([0.0, 0.0, height])))
TLIFT_LH = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))
T0_LH = dot(T0_LIFT, TLIFT_LH)
h.append(misc.DrawAxes(env, T0_LH, 0.4))
TLIFT_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, -dist, 0.0])))
T0_RH = dot(T0_LIFT, TLIFT_RH)
h.append(misc.DrawAxes(env, T0_RH, 0.4))
# Define robot base constraint(s)
# a) Bounds <type 'list'>
# xyz in meters, rpy in radians
# [1.0, 1.0, 1.0, 0.0, 0.0, 0.0] would mean, we allow robot bases
# to be 1 meter apart in each direction but we want them to have the
# same rotation
# Relative Base Constraints between the two robots
#relBaseConstraint = MakeTransform() # This is type (b)
relBaseConstraint = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, 0.0, 0.0])))
# Base Constraints of the first (master) robot in the world coordinate frame
# Same as relative base constraints. First 3 elements are XYZ bounds and the last three elements determine the desired rotation matrix of the base of the master robot in world coords.
masterBaseConstraint = [1.0, 1.0, 0.0, 0.0, 0.0, 0.0]
# b) Exact transform <type 'numpy.ndarray'>
# relBaseConstraint = dot(something, some_other_thing)
# Try to find a valid candidate that satisfies
# all the constraints we have (base location, collision, and configuration-jump)
# Relative transform of initial grasp transforms
TLH_RH = dot(linalg.inv(T0_LH), T0_RH)
print "Ready to find sisters... ", str(datetime.now())
resp = find_sister_pairs(myRmaps, [relBaseConstraint], [TLH_RH], env)
# resp can be None or [pairs, rm, p]
if resp == None:
return resp
else:
return [resp[0], resp[1], [relBaseConstraint], [TLH_RH], T0_LH, T0_RH]
# #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
return masterBaseConstOK
env = Environment()
env.SetViewer('qtcoin')
T0_p = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))
# 1. Create a trajectory for the tool center point to follow
Tstart = array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0]))))
Tgoal = array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.2,0.0]))))
traj = []
traj.append(Tstart)
traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.02,0.0])))))
traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.07,0.0])))))
traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.12,0.0])))))
traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.16,0.0])))))
traj.append(Tgoal)
# Just to try
# Move, then rotate around the axis with that shift being the radius
handles = []
normalsmoothingitrs = 150;
fastsmoothingitrs = 20;
env = Environment()
RaveSetDebugLevel(DebugLevel.Info) # set output level to debug
robotid = env.ReadRobotURI('../../../openHubo/drchubo/robots/drchubo.robot.xml')
crankid = env.ReadRobotURI('../../../../drc_common/models/driving_wheel.robot.xml')
env.Add(robotid)
env.Add(crankid)
env.SetViewer('qtcoin')
shiftUp = 0.95
# Move the wheel infront of the robot
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(env,'CBiRRT')
probs_crankmover = RaveCreateModule(env,'CBiRRT')
manips = robotid.GetManipulators()
crankmanip = crankid.GetManipulators()
try:
env.AddModule(probs_cbirrt,robotid.GetName())
env.AddModule(probs_crankmover,crankid.GetName())
except openrave_exception, e:
print e
print "Getting Loaded Problems"
probs = env.GetLoadedProblems()
env.Add(robots[len(robots)-1]) # add the last robot in the environment so that we can rename it.
for body in env.GetBodies():
rname = body.GetName()
if(rname == 'BarrettWAM'):
newname = 'robot'+str(i)
body.SetName(newname)
print body
# drchubo_manips = drchubo.GetManipulators()
# drchubo_rightFoot = drchubo_manips[3]
# eet1 = robot1.GetEndEffectorTransform() # right foot end effector transform
shift_robot0 = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))
shift_robot1 = MakeTransform(matrix(rodrigues([0,0,pi])),transpose(matrix([-1.0,0.0,0.0])))
robots[0].SetTransform(array(shift_robot0))
robots[1].SetTransform(array(shift_robot1))
mybox = RaveCreateKinBody(env,'')
mybox.SetName('box')
mybox.InitFromBoxes(numpy.array([[-0.5,0,0.3,0.025,0.025,0.3]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3
env.Add(mybox,True)
h=misc.DrawAxes(env,mybox.GetTransform(),1.0)
print "Done! Press Enter to exit..."
sys.stdin.readline()
return self.solved()
def continousSolve(self,itrs=1000,auto=False,extra=[]):
#Enable TSR sampling
self.sample_bw=True
for k in range(itrs):
if not(self.robot.GetEnv().CheckCollision(self.robot)) and not(self.robot.CheckSelfCollision()) and self.solved():
print "Valid solution found!"
#TODO: log solutuon + affine DOF
#rezero to prevent getting stuck...at major speed penalty
self.robot.SetDOFValues(self.zero)
self.run(auto,extra)
time.sleep(.1)
return self.solved()
def resetZero(self):
self.zero=self.robot.GetDOFValues()
if __name__ == '__main__':
juiceTSR = TSR()
juiceTSR.Tw_e = MakeTransform(rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T)
juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi])
juiceTSR.manipindex = 0
test=GeneralIK('','',[juiceTSR],False)
test.supportlinks=['leftFootBase']
test.cogtarget=(.1,.2,.3)
print test.Serialize()
def trans_to_str(T):
myStr = ""
for c in range(0,3):
for r in range(0,3):
myStr += str(T[r,c])+" "
for r in range(0,3):
myStr += str(T[r,3])+" "
return myStr
env = Environment()
env.SetViewer('qtcoin')
h = []
h.append(misc.DrawAxes(env,array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))),1.0))
# Add drchubo
robots = []
# limitless drchubo (old)
robots.append(env.ReadRobotURI('../../../openHubo/drchubo/old/drchubo-urdf/robots/drchubo2.robot.xml'))
# up-to-date drchubo with joint limits defined
robots.append(env.ReadRobotURI('../../../openHubo/drchubo/robots/drchubo2.robot.xml'))
# Which robot?
idx = 0
env.Add(robots[idx])
fastsmoothingitrs = 20
env = Environment()
RaveSetDebugLevel(DebugLevel.Info) # set output level to debug
robotid = env.ReadRobotURI(
'../../../openHubo/huboplus/rlhuboplus_mit.robot.xml')
crankid = env.ReadRobotURI(
'../../../../drc_common/models/driving_wheel.robot.xml')
env.Add(robotid)
env.Add(crankid)
env.SetViewer('qtcoin')
# Move the wheel infront of the robot
crankid.SetTransform(
array(
MakeTransform(
dot(rodrigues([0, 0, pi / 2]), rodrigues([pi / 2, 0, 0])),
transpose(matrix([0.18, 0.09, 0.9])))))
probs_cbirrt = RaveCreateModule(env, 'CBiRRT')
probs_crankmover = RaveCreateModule(env, 'CBiRRT')
manips = robotid.GetManipulators()
crankmanip = crankid.GetManipulators()
try:
env.AddModule(
probs_cbirrt,
'rlhuboplus') # this string should match to kinematic body
env.AddModule(probs_crankmover, 'crank')
except openrave_exception, e:
print e
myDisc.InitFromGeometries([infocylinder]) # we could add more cylinders here
myDisc.SetName('valve')
env.Add(myDisc,True)
env.Add(robotid)
env.Add(crankid)
env.SetViewer('qtcoin')
shiftUp = 1.25
worldPitch = 0.0
tiltDiff = acos(dot(linalg.inv(crankid.GetManipulators()[0].GetEndEffectorTransform()),crankid.GetLinks()[0].GetTransform())[1,1])
# Move the wheel infront of the robot
crankid.SetTransform(array(MakeTransform(dot(rodrigues([0,0,pi/2]),rodrigues([(pi/2-tiltDiff+worldPitch),0,0])),transpose(matrix([0.6, 0.0, shiftUp])))))
myDisc.SetTransform(crankid.GetManipulators()[0].GetTransform())
# sys.stdin.readline()
probs_cbirrt = RaveCreateModule(env,'CBiRRT')
probs_crankmover = RaveCreateModule(env,'CBiRRT')
manips = robotid.GetManipulators()
crankmanip = crankid.GetManipulators()
try:
env.AddModule(probs_cbirrt,robotid.GetName())
env.AddModule(probs_crankmover,crankid.GetName())
except openrave_exception, e:
print e
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 generate_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()
# printself.probs[0] # --> self.probs_cbirrt
# printself.probs[1] # --> self.probs_crankmover
self.robot.SetActiveDOFs([15, 16, 17, 18, 19, 20, 21, 27, 28, 29, 30, 31, 32, 33])
self.initconfig = self.robot.GetActiveDOFValues()
# print"initconfig"
# printself.initconfig
# printtype(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, -pi / 8, 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([pi / 4, 0, 0])), transpose(matrix([0.1, 0.1, -0.2])))
)
self.T0_LH1_APPROACH_2 = dot(
temp, MakeTransform(matrix(rodrigues([pi / 4, 0, 0])), transpose(matrix([0.03, 0.0, -0.15])))
)
self.T0_LH1_ENTRY = dot(
temp, MakeTransform(matrix(rodrigues([pi / 4, 0, 0])), transpose(matrix([-0.01, 0.01, -0.14])))
)
# 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 / 8, 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([-pi / 4, 0, 0])), transpose(matrix([0.1, -0.1, -0.2])))
)
self.T0_RH1_APPROACH_2 = dot(
temp, MakeTransform(matrix(rodrigues([-pi / 4, 0, 0])), transpose(matrix([0.03, 0.0, -0.15])))
)
self.T0_RH1_ENTRY = dot(
temp, MakeTransform(matrix(rodrigues([-pi / 4, 0, 0])), transpose(matrix([-0.01, -0.01, -0.12])))
)
################################################################
## 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", " ")
####################################################################
## 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 not self.is_valid(self.startik, "startik"):
# print"Hey: No IK Solution found! Move the robot!"
# 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 / 4
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 not self.is_valid(self.goalik, "goalik"):
# print"Hey: No IK Solution found! Move the robot!"
# self.notPlanning = True
return []
self.crankid.SetDOFValues([crank_rot], [self.crankjointind])
#########################################################
# This is the end of the second block in Matlab version #
#########################################################
self.crankid.SetDOFValues([self.crankjointind])
# self.notPlanning = True
return 1
for x in range(21):
# x is on the X-Axis of roboground and facing wherever we want it to
TCOM_LF = dot(linalg.inv(T0_COM), T0_LF)
TCOM_RF = dot(linalg.inv(T0_COM), T0_RF)
#Let's shift roboground to where the center of mass is on the ground
# Note TCOMXY is where the COM is but it's rotation is the same with the world
# TODO: Instead of calculating T0_COMXY's rotation from T0_FACING,
# get the torso frame and rotate it back to being flat
# around x and y but don't touch the orientation around z.
T0_COMXY = array(
MakeTransform(T0_FACING[0:3, 0:3],
transpose(matrix([T0_COM[0, 3], T0_COM[1, 3], 0]))))
#TCOMXY_LFTARGET = array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),TCOM_LF[1,3],0.0]))))
TCOMXY_LFTARGET = array(
MakeTransform(rodrigues([0, 0, 0]),
transpose(matrix([((-0.1) + x * 0.01), 0.09, 0.0]))))
#TCOMXY_RFTARGET = array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),TCOM_RF[1,3],0.0]))))
TCOMXY_RFTARGET = array(
MakeTransform(rodrigues([0, 0, 0]),
transpose(matrix([((-0.1) + x * 0.01), -0.09,
0.0]))))
# Now we know where to face and where the feet should be
T0_LFTARGET = dot(T0_COMXY, TCOMXY_LFTARGET)
# T0_LFTARGET = dot(T0_TORSOXY,array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),0.1,0.0])))))
T0_RFTARGET = dot(T0_COMXY, TCOMXY_RFTARGET)
# T0_RFTARGET = dot(T0_TORSOXY,array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),-0.1,0.0])))))
# old code - delete if the code above works
#
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
#set printing, display options, and collision checker
orEnv.SetDebugLevel(DebugLevel.Info)
colchecker = RaveCreateCollisionChecker(orEnv, 'ode')
orEnv.SetCollisionChecker(colchecker)
#create problem instances
probs_cbirrt = RaveCreateProblem(orEnv, 'SOCBiRRT')
orEnv.LoadProblem(probs_cbirrt, 'BarrettWAM')
#TSR chain
#place the first TSR's reference frame at the object's frame relative to world frame
T0_w = T0_object
#get the TSR's offset frame in w coordinates
Tw_e1 = MakeTransform(rodrigues([0, 0, 0]), mat([0, 0.0, -0.05]).T)
#define bounds to only allow rotation of the hand about z axis and a small deviation in translation along the z axis
Bw = mat([0, 0, 0, 0, -0.02, 0.02, -pi / 2, pi / 2, -pi / 2, pi / 2, 0, 0])
TSRstring1 = SerializeTSR(0, 'NULL', T0_w, Tw_e1, Bw)
TSRChainString1 = SerializeTSRChain(0, 1, 0, 1, TSRstring1, 'NULL', [])
cmd = 'RunSoCBiRRT '
cmd = cmd + 'timelimit %d ' % (20)
cmd = cmd + 'goalobject %s ' % ('juice')
cmd = cmd + 'goalvelocitymagnitude %f ' % (0.0025)
cmd = cmd + 'thickness %f ' % (0.25)
cmd = cmd + 'screenshot %d ' % (0)
cmd = cmd + 'mergetree %d ' % (1)
cmd = cmd + 'planning_theta %d ' % (4)
#cmd = cmd + '1.836 100.641 0.032 0.085 '
self.TSRs[0].T0_w = copy.deepcopy(T0_w_in)
def SetFirstTw_e(self,Tw_e_in):
self.TSRs[0].Tw_e = copy.deepcopy(Tw_e_in)
def GetFirstTw_e(self):
return self.TSRs[0].Tw_e
def SetManipIndex(self, manipindex_in):
for i in range(0,len(self.TSRs)):
self.TSRs[i].SetManipIndex(manipindex_in)
if __name__ == '__main__':
juiceTSR = TSR()
juiceTSR.Tw_e = MakeTransform(rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T)
juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi])
juiceTSR.manipindex = 0
juiceTSRChain1 = TSRChain(1,0)
juiceTSRChain1.insertTSR(juiceTSR)
juiceTSR.Tw_e = MakeTransform(rodrigues([0, pi, 0])*rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T)
juiceTSRChain2 = TSRChain(1,0)
juiceTSRChain2.insertTSR(juiceTSR)
print juiceTSRChain1.Serialize()
print juiceTSRChain2.Serialize()
robots = []
# the following for loop will add N robots from the same xml file
# and rename them to prevent failure
for i in range(2):
robots.append(env.ReadRobotURI('robots/barrettwam.robot.xml'))
env.Add(robots[len(robots)-1]) # add the last robot in the environment so that we can rename it.
for body in env.GetBodies():
rname = body.GetName()
if(rname == 'BarrettWAM'):
newname = 'BarrettWAM_'+str(i)
body.SetName(newname)
print body
shift_robot0 = MakeTransform(matrix(rodrigues([0,0,pi])),transpose(matrix([0.5,0.0,0.0])))
shift_robot1 = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([-1.5,0.0,0.0])))
robots[0].SetTransform(array(shift_robot0))
robots[1].SetTransform(array(shift_robot1))
mybox = RaveCreateKinBody(env,'')
mybox.SetName('box')
mybox.InitFromBoxes(numpy.array([[-0.5,0,0.3,0.025,0.025,0.3]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3
env.Add(mybox,True)
handles = []
handles.append(misc.DrawAxes(env,mybox.GetTransform(),1.0))
gmodels = []
