def getNominalPose(self):
axes = transformUtils.getAxesFromTransform(
self.computeGraspFrame().transform)
yaxis = axes[1]
yawDesired = np.arctan2(yaxis[1], yaxis[0])
seedDistance = 1
nominalPose = self.ikPlanner.jointController.getPose('q_nom')
nominalPose[0] = (self.computeGraspFrame().transform.GetPosition()[0] -
seedDistance * yaxis[0])
nominalPose[1] = (self.computeGraspFrame().transform.GetPosition()[1] -
seedDistance * yaxis[1])
nominalPose[5] = yawDesired
if self.scribeDirection == 1: # Clockwise
nominalPose = self.ikPlanner.getMergedPostureFromDatabase(
nominalPose,
'valve',
'reach-nominal-cw',
side=self.graspingHand)
else: # Counter-clockwise
nominalPose = self.ikPlanner.getMergedPostureFromDatabase(
nominalPose,
'valve',
'reach-nominal-ccw',
side=self.graspingHand)
return nominalPose
def planInsertTraj(self, speed, lockFeet=True, lockBase=None, resetBase=False, wristAngleCW=0,
startPose=None, verticalOffset=0.01, usePoses=False, resetPoses=True,
planFromCurrentRobotState=False, retract=False):
ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=speed,
numberOfAddedKnots=1,
quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
fixInitialState=planFromCurrentRobotState)
_, yaxis, _ = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform)
yawDesired = np.arctan2(yaxis[1], yaxis[0])
if startPose is None:
startPose = self.getPlanningStartPose()
nominalPose = self.getNominalPose()
self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
self.ikPlanner.addPose(startPose, self.startPoseName)
self.ikPlanner.reachingSide = self.graspingHand
constraints = []
constraints.extend(self.createBaseConstraints(resetBase, lockBase, lockFeet, yawDesired))
constraints.append(self.createBackPostureConstraint())
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.extend(self.createFootConstraints(lockFeet))
constraints.append(self.ikPlanner.createLockedArmPostureConstraint(self.startPoseName))
constraints.append(self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.createElbowPostureConstraint())
constraints.append(self.createStaticTorqueConstraint())
constraints.append(self.createHandGazeConstraint())
constraints.append(self.createHandFixedOrientConstraint())
constraints.append(self.createWristAngleConstraint(wristAngleCW,
planFromCurrentRobotState))
constraints.extend(self.createAllHandPositionConstraints(self.coaxialTol, retract))
if retract:
startPoseName = self.getStartPoseName(planFromCurrentRobotState, True, usePoses)
endPoseName = self.getEndPoseName(True, usePoses)
endPose = self.ikPlanner.jointController.getPose(endPoseName)
endPose = self.ikPlanner.mergePostures(endPose, robotstate.matchJoints('lwy'), startPose)
endPoseName = 'q_retract'
self.ikPlanner.addPose(endPose, endPoseName)
else:
startPoseName = self.getStartPoseName(planFromCurrentRobotState, retract, usePoses)
endPoseName = self.getEndPoseName(retract, usePoses)
plan = self.ikPlanner.runIkTraj(constraints, startPoseName, endPoseName, self.nominalPoseName, ikParameters=ikParameters)
if resetPoses and not retract and max(plan.plan_info) <= 10:
self.setReachAndTouchPoses(plan)
return plan
def updateCamera(msg):
T = transformUtils.frameFromPositionAndRPY(msg.q[:3], np.degrees(msg.q[3:]))
axes = transformUtils.getAxesFromTransform(T)
# vis.updateFrame(T, 'vicon camera')
# return
camera = app.view.camera()
camera.SetPosition(T.GetPosition())
camera.SetFocalPoint(np.array(T.GetPosition()) + axes[0])
camera.SetViewUp(axes[2])
camera.SetViewAngle(122.6)
app.view.render()
def updateCamera(msg):
T = transformUtils.frameFromPositionAndRPY(msg.q[:3],np.degrees(msg.q[3:]))
axes = transformUtils.getAxesFromTransform(T)
#vis.updateFrame(T, 'vicon camera')
#return
camera = app.view.camera()
camera.SetPosition(T.GetPosition())
camera.SetFocalPoint(np.array(T.GetPosition())+axes[0])
camera.SetViewUp(axes[2])
camera.SetViewAngle(122.6)
app.view.render()
def getNominalPose(self):
axes = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform)
yaxis = axes[1]
yawDesired = np.arctan2(yaxis[1], yaxis[0])
seedDistance = 1
nominalPose = self.ikPlanner.jointController.getPose('q_nom')
nominalPose[0] = (self.computeGraspFrame().transform.GetPosition()[0] -
seedDistance*yaxis[0])
nominalPose[1] = (self.computeGraspFrame().transform.GetPosition()[1] -
seedDistance*yaxis[1])
nominalPose[5] = yawDesired
if self.scribeDirection == 1: # Clockwise
nominalPose = self.ikPlanner.getMergedPostureFromDatabase(nominalPose, 'valve', 'reach-nominal-cw', side=self.graspingHand)
else: # Counter-clockwise
nominalPose = self.ikPlanner.getMergedPostureFromDatabase(nominalPose, 'valve', 'reach-nominal-ccw', side=self.graspingHand)
return nominalPose
def testTransform():
'''
test transformFromPose --> getAxesFromTransform is same as quat --> matrix
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
axes = transformUtils.getAxesFromTransform(frame)
mat = transformUtils.getNumpyFromTransform(frame)
assert np.allclose(mat[:3, :3], np.array(axes).transpose())
mat2 = transformations.quaternion_matrix(quat)
mat2[:3, 3] = pos
print mat
print mat2
assert np.allclose(mat, mat2)
def createUtorsoGazeConstraints(self, tspan):
constraints = []
g = ik.WorldGazeDirConstraint()
g.linkName = 'utorso'
g.targetFrame = vtk.vtkTransform()
axes = transformUtils.getAxesFromTransform(self.polaris.leftFootEgressOutsideFrame.transform)
g.targetAxis = axes[0]
g.bodyAxis = [1,0,0]
g.coneThreshold = self.coneThreshold
g.tspan = tspan
constraints.append(g)
g = ik.WorldGazeDirConstraint()
g.linkName = 'utorso'
g.targetFrame = vtk.vtkTransform()
g.targetAxis = [0,0,1]
g.bodyAxis = [0,0,1]
g.coneThreshold = self.coneThreshold
g.tspan = tspan
constraints.append(g)
return constraints
def addCollisionObject(self, obj):
if om.getOrCreateContainer('affordances').findChild(obj.getProperty('Name') + ' affordance'):
return # Affordance has been created previously
frame = obj.findChild(obj.getProperty('Name') + ' frame')
(origin, quat) = transformUtils.poseFromTransform(frame.transform)
(xaxis, yaxis, zaxis) = transformUtils.getAxesFromTransform(frame.transform)
# TODO: move this into transformUtils as getAxisDimensions or so
box = obj.findChild(obj.getProperty('Name') + ' box')
box_np = vtkNumpy.getNumpyFromVtk(box.polyData, 'Points')
box_min = np.amin(box_np, 0)
box_max = np.amax(box_np, 0)
xwidth = np.linalg.norm(box_max[0]-box_min[0])
ywidth = np.linalg.norm(box_max[1]-box_min[1])
zwidth = np.linalg.norm(box_max[2]-box_min[2])
name = obj.getProperty('Name') + ' affordance'
boxAffordance = segmentation.createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name, parent='affordances')
boxAffordance.setSolidColor(obj.getProperty('Color'))
boxAffordance.setProperty('Alpha', 0.3)
def getCorners(self):
'''
Return a 4x3 numpy array representing the world xyz positions of the
four corners of the block top. Corners are listed clockwise from far right.
'''
width = self.rectWidth
depth = self.rectDepth
width = max(width, 0.39)
#depth = max(depth, 0.38)
xaxis, yaxis, zaxis = transformUtils.getAxesFromTransform(self.cornerTransform)
xedge = np.array(xaxis)*depth
yedge = np.array(yaxis)*width
c1 = np.array(self.cornerTransform.GetPosition()) + (np.array(yaxis)*self.rectWidth*0.5) - yedge*0.5
c2 = c1 - xedge
c3 = c1 - xedge + yedge
c4 = c1 + yedge
return np.array([c3, c4, c1, c2])
def getCorners(self):
'''
Return a 4x3 numpy array representing the world xyz positions of the
four corners of the block top. Corners are listed clockwise from far right.
'''
width = self.rectWidth
depth = self.rectDepth
width = max(width, 0.39)
#depth = max(depth, 0.38)
xaxis, yaxis, zaxis = transformUtils.getAxesFromTransform(self.cornerTransform)
xedge = np.array(xaxis)*depth
yedge = np.array(yaxis)*width
c1 = np.array(self.cornerTransform.GetPosition()) + (np.array(yaxis)*self.rectWidth*0.5) - yedge*0.5
c2 = c1 - xedge
c3 = c1 - xedge + yedge
c4 = c1 + yedge
return np.array([c3, c4, c1, c2])
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
d.addLine(originProjectedToPlane, origin, color=[0,1,0])
d.addLine(originProjectedToEdge, origin, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
def planInsertTraj(self,
speed,
lockFeet=True,
lockBase=None,
resetBase=False,
wristAngleCW=0,
startPose=None,
verticalOffset=0.01,
usePoses=False,
resetPoses=True,
planFromCurrentRobotState=False,
retract=False):
ikParameters = IkParameters(
usePointwise=False,
maxDegreesPerSecond=speed,
numberOfAddedKnots=1,
quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
fixInitialState=planFromCurrentRobotState)
_, yaxis, _ = transformUtils.getAxesFromTransform(
self.computeGraspFrame().transform)
yawDesired = np.arctan2(yaxis[1], yaxis[0])
if startPose is None:
startPose = self.getPlanningStartPose()
nominalPose = self.getNominalPose()
self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
self.ikPlanner.addPose(startPose, self.startPoseName)
self.ikPlanner.reachingSide = self.graspingHand
constraints = []
constraints.extend(
self.createBaseConstraints(resetBase, lockBase, lockFeet,
yawDesired))
constraints.append(self.createBackPostureConstraint())
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.extend(self.createFootConstraints(lockFeet))
constraints.append(
self.ikPlanner.createLockedArmPostureConstraint(
self.startPoseName))
constraints.append(
self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.createElbowPostureConstraint())
constraints.append(self.createStaticTorqueConstraint())
constraints.append(self.createHandGazeConstraint())
constraints.append(self.createHandFixedOrientConstraint())
constraints.append(
self.createWristAngleConstraint(wristAngleCW,
planFromCurrentRobotState))
constraints.extend(
self.createAllHandPositionConstraints(self.coaxialTol, retract))
if retract:
startPoseName = self.getStartPoseName(planFromCurrentRobotState,
True, usePoses)
endPoseName = self.getEndPoseName(True, usePoses)
endPose = self.ikPlanner.jointController.getPose(endPoseName)
endPose = self.ikPlanner.mergePostures(
endPose, robotstate.matchJoints('lwy'), startPose)
endPoseName = 'q_retract'
self.ikPlanner.addPose(endPose, endPoseName)
else:
startPoseName = self.getStartPoseName(planFromCurrentRobotState,
retract, usePoses)
endPoseName = self.getEndPoseName(retract, usePoses)
plan = self.ikPlanner.runIkTraj(constraints,
startPoseName,
endPoseName,
self.nominalPoseName,
ikParameters=ikParameters)
if resetPoses and not retract and max(plan.plan_info) <= 10:
self.setReachAndTouchPoses(plan)
return plan
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame,
[1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData,
'running board search points',
parent=segmentation.getDebugFolder(),
color=[0, 1, 0],
visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints,
'edge points',
parent=segmentation.getDebugFolder(),
visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
[1.0, 1.0])
dists = np.dot(
vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
vis.updatePolyData(fitPoints,
'line fit points',
parent=segmentation.getDebugFolder(),
colorByName='ransac_labels',
visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection * np.dot(
origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(
originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(),
'running board edge',
parent=segmentation.getDebugFolder(),
colorByName='RGB255',
visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(
xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
box.getChildFrame().copyFrame(t)
self.initialize()