def computeBoardStanceFrame(self):
objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform )
self.relativeStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeStanceXYZ , self.relativeStanceRPY ) )
robotStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeStanceTransform )
self.stanceFrame = vis.updateFrame(robotStance, 'board stance', parent=self.affordance, visible=False, scale=0.2)
self.stanceFrame.addToView(app.getDRCView())
objectTransform = transformUtils.copyFrame( self.graspLeftFrame.transform )
self.relativeAsymmetricStanceTransform = transformUtils.copyFrame( transformUtils.frameFromPositionAndRPY( self.relativeAsymmetricStanceXYZ , self.relativeStanceRPY ) )
robotAsymmetricStance = self.b.computeRobotStanceFrame( objectTransform, self.relativeAsymmetricStanceTransform )
self.asymmetricStanceFrame = vis.updateFrame(robotAsymmetricStance, 'board Asymmetric stance', parent=self.affordance, visible=False, scale=0.2)
self.asymmetricStanceFrame.addToView(app.getDRCView())
def computeBoardGraspFrames(self):
t = transformUtils.frameFromPositionAndRPY( self.graspLeftHandFrameXYZ , self.graspLeftHandFrameRPY )
t_copy = transformUtils.copyFrame(t)
t_copy.Concatenate(self.frame.transform)
self.graspLeftFrame = vis.updateFrame(t_copy, 'grasp left frame', parent=self.affordance, visible=False, scale=0.2)
self.graspLeftFrame.addToView(app.getDRCView())
t = transformUtils.frameFromPositionAndRPY( self.graspRightHandFrameXYZ , self.graspRightHandFrameRPY )
t_copy = transformUtils.copyFrame(t)
t_copy.Concatenate(self.frame.transform)
self.graspRightFrame = vis.updateFrame(t_copy, 'grasp right frame', parent=self.affordance, visible=False, scale=0.2)
self.graspRightFrame.addToView(app.getDRCView())
def makeGoalFrames(self):
for linkName, positionGoal, _ in self.positionCosts:
orientationGoal = [0.0, 0.0, 0.0]
t = transformUtils.frameFromPositionAndRPY(positionGoal, orientationGoal)
f = vis.showFrame(t, "%s position goal" % linkName)
f.connectFrameModified(self.onGoalFrameModified)
def getFeetMidPoint(model, useWorldZ=True):
'''
Returns a frame in world coordinate system that is the average of the left
and right foot reference point positions in world frame, the average of the
left and right foot yaw in world frame, and Z axis aligned with world Z.
The foot reference point is the average of the foot contact points in the foot frame.
'''
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame
contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame
t_lf_mid = model.getLinkFrame(_leftFootLink)
t_lf_mid.PreMultiply()
t_lf_mid.Translate(contact_pts_mid_left)
t_rf_mid = model.getLinkFrame(_rightFootLink)
t_rf_mid.PreMultiply()
t_rf_mid.Translate(contact_pts_mid_right)
if "atlas" in _modelName: # atlas_v3/v4/v5
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
elif (_modelName == "valkyrie"): # valkyrie
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
else:
raise ValueError("Model Name not recognised")
if useWorldZ:
rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])]
return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy)
else:
return t_feet_mid
def projectDrillDemoInCamera():
q = om.findObjectByName('camera overlay')
om.removeFromObjectModel(q)
imageView = cameraview.views['CAMERA_LEFT']
imageView.imageActor.SetOpacity(.2)
drawFrameInCamera(drillDemo.drill.frame.transform, 'drill frame',visible=False)
tf = transformUtils.copyFrame( drillDemo.drill.frame.transform )
tf.PreMultiply()
tf.Concatenate( drillDemo.drill.drillToButtonTransform )
drawFrameInCamera(tf, 'drill button')
tf2 = transformUtils.copyFrame( tf )
tf2.PreMultiply()
tf2.Concatenate( transformUtils.frameFromPositionAndRPY( [0,0,0] , [180,0,0] ) )
drawFrameInCamera(tf2, 'drill button flip')
drawObjectInCamera('drill',visible=False)
drawObjectInCamera('sensed pointer tip')
obj = om.findObjectByName('sensed pointer tip frame')
if (obj is not None):
drawFrameInCamera(obj.transform, 'sensed pointer tip frame',visible=False)
#drawObjectInCamera('left robotiq',visible=False)
#drawObjectInCamera('right pointer',visible=False)
v = imageView.view
v.render()
def __init__(self, jointController):
self.jointController = jointController
pos, rpy = jointController.q[:3], jointController.q[3:6]
t = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy))
self.frame = vis.showFrame(t, 'mover widget', scale=0.3)
self.frame.setProperty('Edit', True)
self.frame.connectFrameModified(self.onFrameModified)
def planStandUp(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
pelvisFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('pelvis', startPose)
t = transformUtils.frameFromPositionAndRPY([self.pelvisLiftX, 0, self.pelvisLiftZ], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([t, pelvisFrame])
constraints = []
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
g = self.createUtorsoGazeConstraints([1.0, 1.0])
constraints.append(g[1])
p = ik.PositionConstraint(linkName='pelvis', referenceFrame=liftFrame,
lowerBound=np.array([0.0, -np.inf, 0.0]),
upperBound=np.array([np.inf, np.inf, 0.0]))
constraints.append(p)
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
constraints.extend(self.robotSystem.ikPlanner.createFixedFootConstraints(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=True)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot', 'l_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan
def spawnBoardAffordance(self, randomize=False):
self.boardLength = 1.5
if randomize:
position = [random.uniform(0.5, 0.8), random.uniform(-0.2, 0.2), random.uniform(0.5, 0.8)]
rpy = [random.choice((random.uniform(-35, 35), random.uniform(70, 110))), random.uniform(-10, 10), random.uniform(-5, 5)]
zwidth = random.uniform(0.5, 1.0)
else:
if self.b.val:
position = [0.4, 0.0, 1.]
else:
position = [0.6, 0.0, 1.]
rpy = [90, 1, 0]
zwidth = self.boardLength
xwidth = 3.75 * .0254
ywidth = 1.75 * .0254
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t.Concatenate(self.b.computeGroundFrame(self.b.robotModel))
xaxis = [1,0,0]
yaxis = [0,1,0]
zaxis = [0,0,1]
for axis in (xaxis, yaxis, zaxis):
t.TransformVector(axis, axis)
self.affordance = segmentation.createBlockAffordance(t.GetPosition(), xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, 'board', parent='affordances')
self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100))
t = self.affordance.actor.GetUserTransform()
self.frame = vis.showFrame(t, 'board frame', parent=self.affordance, visible=False, scale=0.2)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
print 'have existing widget for step:', stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
print 'returning because widget was for selected step'
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def __init__(self, channel='OPTITRACK_FRAMES', name='Optitrack Visualier'):
self.name = name
self.channel = channel
self.subscriber = None
self.optitrackToWorld = transformUtils.frameFromPositionAndRPY([0,0,0],[90,0,90])
self.subscriber = None
self.fpsCounter = FPSCounter()
self.fpsCounter.printToConsole = False
def planFootOut(self, startPose=None, finalFootHeight=0.05):
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, finalFootHeight)
constraints = []
constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0]))
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True,
pelvisEnabled=False, shrinkFactor=0.01))
constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint())
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
#constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot'))
constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10,
rescaleBodyNames=['l_foot'],
rescaleBodyPts=[0.0, 0.0, 0.0],
maxBodyTranslationSpeed=self.maxFootTranslationSpeed)
#constraintSet.seedPoseName = 'q_start'
#constraintSet.nominalPoseName = 'q_start'
constraintSet.runIk()
footFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('l_foot', startPose)
t = transformUtils.frameFromPositionAndRPY([0, 0, self.polaris.leftFootEgressOutsideFrame.transform.GetPosition()[2]-footFrame.GetPosition()[2]], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([footFrame, t])
vis.updateFrame(liftFrame, 'lift frame')
c = ik.WorldFixedOrientConstraint()
c.linkName = 'l_foot'
c.tspan = [0.0, 0.1, 0.2]
constraints.append(c)
constraints.extend(self.createLeftFootPoseConstraint(liftFrame, tspan=[0.2, 0.2]))
constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressMidFrame, tspan=[0.5, 0.5]))
constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressOutsideFrame, tspan=[0.8, 0.8]))
#plan = constraintSet.planEndPoseGoal(feetOnGround=False)
keyFramePlan = constraintSet.runIkTraj()
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, False)
self.addPlan(plan)
return plan
def handleStatus(self, msg):
if msg.plan_type == msg.STANDING:
goal = transformUtils.frameFromPositionAndRPY(
np.array([robotStateJointController.q[0] + 2 * self.max_distance_per_plan * (np.random.random() - 0.5),
robotStateJointController.q[1] + 2 * self.max_distance_per_plan * (np.random.random() - 0.5),
robotStateJointController.q[2] - 0.84]),
[0, 0, robotStateJointController.q[5] + 2 * np.degrees(np.pi) * (np.random.random() - 0.5)])
request = footstepsDriver.constructFootstepPlanRequest(robotStateJointController.q, goal)
request.params.max_num_steps = 18
footstepsDriver.sendFootstepPlanRequest(request)
def computeBoardReachFrames(self):
''' Reach ~10cm short of the grasp frame '''
reachLeftXYZ = copy.deepcopy( self.graspLeftHandFrameXYZ )
reachLeftXYZ[0] = reachLeftXYZ[0] - self.reachDepth
reachLeftRPY = copy.deepcopy ( self.graspLeftHandFrameRPY )
tl = transformUtils.frameFromPositionAndRPY( reachLeftXYZ , reachLeftRPY )
tl.Concatenate(self.frame.transform)
self.reachLeftFrame = vis.updateFrame(tl, 'reach left frame', parent=self.affordance, visible=False, scale=0.2)
self.reachLeftFrame.addToView(app.getDRCView())
reachRightXYZ = copy.deepcopy( self.graspRightHandFrameXYZ )
reachRightXYZ[0] = reachRightXYZ[0] - self.reachDepth
reachRightRPY = copy.deepcopy ( self.graspRightHandFrameRPY )
tr = transformUtils.frameFromPositionAndRPY( reachRightXYZ , reachRightRPY )
tr.Concatenate(self.frame.transform)
self.reachRightFrame = vis.updateFrame(tr, 'reach right frame', parent=self.affordance, visible=False, scale=0.2)
self.reachRightFrame.addToView(app.getDRCView())
def planGraspLift(self):
t3 = transformUtils.frameFromPositionAndRPY( [0.00,0.0, 0.04] , [0,0,0] )
liftTransform = transformUtils.copyFrame(self.board.graspFrame.transform)
liftTransform.Concatenate(t3)
self.board.liftFrame = vis.updateFrame(liftTransform, 'lift frame', parent="affordances", visible=False, scale=0.2)
startPose = self.getPlanningStartPose()
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, self.board.liftFrame, lockBase=self.lockBase, lockBack=self.lockBack)
endPose, info = constraintSet.runIk()
liftPlan = constraintSet.runIkTraj()
self.addPlan(liftPlan)
def getFeetMidPoint(model, useWorldZ=True):
'''
Returns a frame in world coordinate system that is the average of the left
and right foot reference point positions in world frame, the average of the
left and right foot yaw in world frame, and Z axis aligned with world Z.
The foot reference point is the average of the foot contact points in the foot frame.
'''
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame
contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame
t_lf_mid = model.getLinkFrame(_leftFootLink)
t_lf_mid.PreMultiply()
t_lf_mid.Translate(contact_pts_mid_left)
t_rf_mid = model.getLinkFrame(_rightFootLink)
t_rf_mid.PreMultiply()
t_rf_mid.Translate(contact_pts_mid_right)
if (_robotType == 0): # Atlas
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
elif (_robotType == 1):
# Valkyrie v1 Foot orientation is silly
l_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [180.0, 82.5, 0])
l_foot_sole.PostMultiply()
l_foot_sole.Concatenate(t_lf_mid)
r_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [0.0, -82.5, 0])
r_foot_sole.PostMultiply()
r_foot_sole.Concatenate(t_rf_mid)
t_feet_mid = transformUtils.frameInterpolate(l_foot_sole, r_foot_sole, 0.5)
elif (_robotType == 2):
# Valkyrie v2 is better
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
if useWorldZ:
rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])]
return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy)
else:
return t_feet_mid
def initCameraFrustumVisualizer(depthScanner):
cameraObj = vis.showPolyData(vtk.vtkPolyData(), 'camera', parent=depthScanner.parentFolder, view=depthScanner.pointCloudView)
cameraFrame = vis.addChildFrame(cameraObj)
cameraFrame.setProperty('Scale', 50)
cameraObj.setProperty('Visible', False)
pointCloudToCamera = transformUtils.frameFromPositionAndRPY((0,0,0,), (-90, 90, 0)).GetLinearInverse()
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def onCameraModified():
cameraToWorld = getCameraTransform(depthScanner.view.camera())
depthScanner.pointCloudObj.actor.SetUserTransform(transformUtils.concatenateTransforms([pointCloudToCamera, cameraToWorld]))
cameraFrame.copyFrame(cameraToWorld)
def enableFrustum():
updateCameraMesh()
cameraObj.setProperty('Visible', True)
onCameraModified()
depthScanner._updateFunc = onCameraModified
applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True)
applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView)
depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])
def disableFrustum():
cameraObj.setProperty('Visible', False)
depthScanner.pointCloudObj.actor.SetUserTransform(None)
depthScanner._updateFunc = None
applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False)
applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView)
depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])
return FieldContainer(
updateCameraMesh=updateCameraMesh,
onCameraModified=onCameraModified,
enableFrustum=enableFrustum,
disableFrustum=disableFrustum
)
def getSpawnFrame(self):
if self.jointController:
# get spawn frame in front of robot
pos = self.jointController.q[:3]
rpy = np.degrees(self.jointController.q[3:6])
frame = transformUtils.frameFromPositionAndRPY(pos, rpy)
frame.PreMultiply()
frame.Translate(0.5, 0.0, 0.3)
else:
frame = vtk.vtkTransform()
return frame
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('shelf item'))
obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
segmentation.makeMovable(obj, t)
def createSpheres(self, sensorValues):
d = DebugData()
for key in sensorValues.keys():
frame, pos, rpy = self.sensorLocations[key]
t = transformUtils.frameFromPositionAndRPY(pos, rpy)
t.PostMultiply()
t.Concatenate(self.frames[frame])
d.addSphere(t.GetPosition(),
radius=0.005,
color=self.getColor(sensorValues[key], key),
resolution=8)
vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
def testEulerToFrame():
'''
Test some euler converions
'''
rpy = transformations.euler_from_quaternion(transformations.random_quaternion())
frame = transformUtils.frameFromPositionAndRPY([0,0,0], np.degrees(rpy))
mat = transformUtils.getNumpyFromTransform(frame)
mat2 = transformations.euler_matrix(rpy[0], rpy[1], rpy[2])
print mat
print mat2
assert np.allclose(mat, mat2)
def getRoomSweepFrames(self, rotateHandFrame=False):
topFrame = transformUtils.frameFromPositionAndRPY([0.65,0.0,0.8],[160,0,90])
yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees])
if rotateHandFrame:
fixHandFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,-90,0])
topFrame.PreMultiply()
topFrame.Concatenate( fixHandFrame )
topFrame.PostMultiply()
topFrame.Concatenate( yawFrame )
bottomFrame = transformUtils.frameFromPositionAndRPY([0.6,0.0,0.4],[210,0,90])
yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees])
if rotateHandFrame:
bottomFrame.PreMultiply()
bottomFrame.Concatenate( fixHandFrame )
bottomFrame.PostMultiply()
bottomFrame.Concatenate( yawFrame )
if (self.fromTop):
self.startFrame = vis.showFrame(topFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder)
self.endFrame = vis.showFrame(bottomFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder)
else:
self.startFrame = vis.showFrame(bottomFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder)
self.endFrame = vis.showFrame(topFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder)
def __init__(self, robotModel, playbackRobotModel, teleopRobotModel, footstepPlanner, manipPlanner, ikPlanner,
lhandDriver, rhandDriver, atlasDriver, multisenseDriver, sensorJointController,
planPlaybackFunction, showPoseFunction):
self.robotModel = robotModel
self.playbackRobotModel = playbackRobotModel # not used inside the demo
self.teleopRobotModel = teleopRobotModel # not used inside the demo
self.footstepPlanner = footstepPlanner
self.manipPlanner = manipPlanner
self.ikPlanner = ikPlanner
self.lhandDriver = lhandDriver
self.rhandDriver = rhandDriver
self.atlasDriver = atlasDriver
self.multisenseDriver = multisenseDriver
self.sensorJointController = sensorJointController
self.planPlaybackFunction = planPlaybackFunction
self.showPoseFunction = showPoseFunction
self.goalTransform1 = transformUtils.frameFromPositionAndRPY([1,0,0],[0,0,0])
self.goalTransform2 = transformUtils.frameFromPositionAndRPY([2,0,0],[0,0,10])
#self.goalTransform2 = transformUtils.frameFromPositionAndRPY([1,1,0],[0,0,90])
self.visOnly = False # True for development, False for operation
self.planFromCurrentRobotState = True # False for development, True for operation
useDevelopment = False
if (useDevelopment):
self.visOnly = True # True for development, False for operation
self.planFromCurrentRobotState = False # False for development, True for operation
self.optionalUserPromptEnabled = False
self.requiredUserPromptEnabled = True
self.constraintSet = None
self.plans = []
self._setupSubscriptions()
def findSafeRegion(self, pose):
pose = np.asarray(pose)
tformForProjection = transformUtils.frameFromPositionAndRPY([0,0,0], pose[3:] * 180 / np.pi)
tform = transformUtils.frameFromPositionAndRPY(pose[:3], pose[3:] * 180 / np.pi)
contact_pts_on_plane = np.zeros((2, self.bot_pts.shape[1]))
for j in range(self.bot_pts.shape[1]):
contact_pts_on_plane[:,j] = tformForProjection.TransformPoint([self.bot_pts[0,j], self.bot_pts[1,j], 0])[:2]
Rdot = np.array([[0, -1], [1, 0]])
contact_vel_in_world = Rdot.dot(contact_pts_on_plane)
c_region = {'A': [], 'b': []}
for i in range(self.planar_polyhedron.A.shape[0]):
ai = self.planar_polyhedron.A[i,:]
n = np.linalg.norm(ai)
ai = ai / n
bi = self.planar_polyhedron.b[i] / n
p = ai.dot(contact_pts_on_plane)
v = ai.dot(contact_vel_in_world)
mask = np.logical_or(p >= 0, v >= 0)
for j, tf in enumerate(mask):
if tf:
c_region['A'].append(np.hstack((ai, v[j])))
c_region['b'].append([bi - p[j]])
A = np.vstack(c_region['A'])
b = np.hstack(c_region['b'])
b = b + A.dot(np.array([0,0,pose[5]]))
self.c_space_polyhedron = Hrep(A, b)
return SafeTerrainRegion(A, b, [], [], tform)
def newWalkingGoal(displayPoint, view):
footFrame = footstepsDriver.getFeetMidPoint(robotModel)
worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
groundOrigin = footFrame.GetPosition()
groundNormal = [0.0, 0.0, 1.0]
selectedGroundPoint = [0.0, 0.0, 0.0]
t = vtk.mutable(0.0)
vtk.vtkPlane.IntersectWithLine(worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint)
walkingTarget = transformUtils.frameFromPositionAndRPY(selectedGroundPoint, np.array(footFrame.GetOrientation()))
footstepsdriverpanel.panel.onNewWalkingGoal(walkingTarget)
def run(self):
inputFrame = self.getInputFrame()
translation = self.properties.getProperty('Translation')
rpy = self.properties.getProperty('Rotation')
offset = transformUtils.frameFromPositionAndRPY(translation, rpy)
offset.PostMultiply()
offset.Concatenate(transformUtils.copyFrame(inputFrame.transform))
outputFrame = vis.updateFrame(offset, self.properties.getProperty('Frame output name'), scale=0.2, parent=inputFrame.parent())
if not hasattr(inputFrame, 'frameSync'):
inputFrame.frameSync = vis.FrameSync()
inputFrame.frameSync.addFrame(inputFrame)
inputFrame.frameSync.addFrame(outputFrame, ignoreIncoming=True)
def makeStepFrames(stepFrames, relativeFrame=None, showFrames=False):
frames = []
for i, stepFrame in enumerate(stepFrames):
stepFrame = transformUtils.frameFromPositionAndRPY(stepFrame, [0,0,0])
stepFrame.PostMultiply()
if relativeFrame:
stepFrame.Concatenate(relativeFrame)
if showFrames:
obj = vis.updateFrame(stepFrame, 'step frame %d' % i, parent='step frames', scale=0.2)
stepFrame = obj.transform
frames.append(stepFrame)
return frames
def onIkStartup(ikServer, startSuccess):
side = 'left'
goalFrame = transformUtils.frameFromPositionAndRPY([0.5, 0.5, 1.2], [0, 90, -90])
assert not checkGraspFrame(goalFrame, side)
frame = teleopPanel.endEffectorTeleop.newReachTeleop(goalFrame, side)
assert checkGraspFrame(goalFrame, side)
teleopPanel.ui.planButton.click()
assert playbackPanel.plan is not None
teleopPanel.ikPlanner.useCollision = True;
teleopPanel.ui.planButton.click()
assert playbackPanel.plan is not None
frame.setProperty('Edit', True)
app.startTestingModeQuitTimer()
def getFootstepToWorldTransforms(self,footstepIdx, stepOffDirection='sideways'):
self.updateFramesAndAffordance()
footstepsToWorldList = []
for j in footstepIdx:
if stepOffDirection == 'sideways':
rpy = np.array([0,0,self.footstepYaw[j]])
position = self.footstepPosition[j]
else:
rpy = np.array([0,0,0], self.footstepYawForwards[j])
position = self.footstepPositionForwards[j]
footstepToPlan = transformUtils.frameFromPositionAndRPY(position,rpy)
footstepToWorld = footstepToPlan
footstepToWorld.PostMultiply();
footstepToWorld.Concatenate(self.planToWorld)
footstepsToWorldList.append(footstepToWorld)
return footstepsToWorldList
def drawModel(self, model):
modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder())
markerFolder = om.getOrCreateContainer("markers", parentObj=modelFolder)
modelName = model.name
markerObjects = markerFolder.children()
if len(markerObjects) != model.nummarkers:
for obj in markerObjects:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor)
self.models[modelName] = markerObjects
if len(markerObjects):
modelColor = markerObjects[0].getProperty("Color")
for i, marker in enumerate(model.markers):
xyz = np.array(marker.xyz) * self.unitConversion
markerFrame = vtk.vtkTransform()
markerFrame.Translate(xyz)
markerObjects[i].getChildFrame().copyFrame(markerFrame)
if self.drawEdges:
d = DebugData()
for m1 in model.markers:
xyz = np.array(m1.xyz) * self.unitConversion
for m2 in model.markers:
xyz2 = np.array(m2.xyz) * self.unitConversion
d.addLine(xyz, xyz2)
edges = shallowCopy(d.getPolyData())
vis.updatePolyData(edges, modelName + " edges", color=modelColor, parent=modelFolder)
else:
edgesObj = om.findObjectByName(modelName + " edges")
om.removeFromObjectModel(edgesObj)
computeModelFrame = True
if computeModelFrame:
pos = np.array(model.segments[0].T) * self.unitConversion
rpy = np.array(model.segments[0].A)
modelFrame = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy))
vis.updateFrame(modelFrame, modelName + " frame", parent=modelFolder, scale=0.1)
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6,0.2,0.6],[180,0,90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0,0,0]), np.array([-self.boxLength,0,0]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,0]), np.array([-self.boxLength,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,self.boxLength]), np.array([0,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([0,0,self.boxLength]), np.array([0,0,0]), radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength, otdf_type='target', friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def planFootOut(self, startPose=None, finalFootHeight=0.05):
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose(
'utorso', startPose)
finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(
startPose, finalFootHeight)
constraints = []
constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0]))
constraints.append(
ikconstraints.QuasiStaticConstraint(leftFootEnabled=False,
rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=0.01))
constraints.append(
self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint())
constraints.append(
self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(
startPoseName))
constraints.append(
self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(
startPoseName))
#constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName))
constraints.append(
self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(
self.robotSystem.ikPlanner.createFixedLinkConstraints(
startPoseName, 'r_foot'))
constraints.extend(
self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,
1]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints,
endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(
usePointwise=True,
maxBaseRPYDegreesPerSecond=10,
rescaleBodyNames=['l_foot'],
rescaleBodyPts=[0.0, 0.0, 0.0],
maxBodyTranslationSpeed=self.maxFootTranslationSpeed)
#constraintSet.seedPoseName = 'q_start'
#constraintSet.nominalPoseName = 'q_start'
constraintSet.runIk()
footFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose(
'l_foot', startPose)
t = transformUtils.frameFromPositionAndRPY([
0, 0,
self.polaris.leftFootEgressOutsideFrame.transform.GetPosition()[2]
- footFrame.GetPosition()[2]
], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([footFrame, t])
vis.updateFrame(liftFrame, 'lift frame')
c = ikconstraints.WorldFixedOrientConstraint()
c.linkName = 'l_foot'
c.tspan = [0.0, 0.1, 0.2]
constraints.append(c)
constraints.extend(
self.createLeftFootPoseConstraint(liftFrame, tspan=[0.2, 0.2]))
constraints.extend(
self.createLeftFootPoseConstraint(
self.polaris.leftFootEgressMidFrame, tspan=[0.5, 0.5]))
constraints.extend(
self.createLeftFootPoseConstraint(
self.polaris.leftFootEgressOutsideFrame, tspan=[0.8, 0.8]))
#plan = constraintSet.planEndPoseGoal(feetOnGround=False)
keyFramePlan = constraintSet.runIkTraj()
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts,
False)
self.addPlan(plan)
return plan
def pointPickerStoredFootsteps(self, p1, p2):
yaw = math.atan2(p2[1] - p1[1], p2[0] - p1[0]) * 180 / math.pi + 90
frame_p1 = transformUtils.frameFromPositionAndRPY(p1, [0, 0, yaw])
blockl = 0.3937
blockh = 0.142875
sep = 0.11
frame_pt_to_centerline = transformUtils.frameFromPositionAndRPY(
[0, -blockl / 2, 0], [0, 0, 0])
frame_pt_to_centerline.PostMultiply()
frame_pt_to_centerline.Concatenate(frame_p1)
vis.updateFrame(frame_pt_to_centerline, "corner", parent="navigation")
# up/down 1 block (Original)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.06 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*1.5 +0.03 ,-.1 , 0 , 0 , 0 , 0]])
# up/down 1 block (Newer)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.04 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*1.5 + 0.00 ,-.1 , 0 , 0 , 0 , 0]])
# up 3 blocks
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] ,
# [ blockl*2.5 + 0.0 ,-.1 , 3*blockh, 0 , 0 , 0]])
# up and down 3 blocks (original)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] ,
# [ blockl*2.5 + 0.03 ,-.1 , 3*blockh, 0 , 0 , 0],
# [ blockl*3.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*3.5 + 0.03 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*4.5 - 0.03 , .1 , 1*blockh, 0 , 0 , 0] ,
# [ blockl*4.5 + 0.03 ,-.1 , 1*blockh, 0 , 0 , 0],
# [ blockl*5.5 - 0.03 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*5.5 + 0.03 ,-.1 , 0 , 0 , 0 , 0]])
# up and down 3 blocks (used in video)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.0 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r],
# [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*5.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half
# [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner
# [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order
# [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r], # top
# [ blockl*2.5 - 0.06 , sep , 3*blockh, 0 , 0 , 0, 0], # top
# [ blockl*1.5 + 0.04 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]])
# up and down 2 blocks (used in vicon april 2014)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r], # start
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r], # 1
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # 2
# [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # 1d
# [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half
# [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner
# [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order
# [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]])
# up 6 blocks (used in journal paper in oct 2014)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 ,-sep , 0 , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 - 0.02 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 - 0.02 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 - 0.02 ,-sep , 3*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 4*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 - 0.02 ,-sep , 4*blockh, 0 , 0 , 0, r],
# [ blockl*4.5 - 0.03 , sep , 5*blockh, 0 , 0 , 0, 0],
# [ blockl*4.5 - 0.02 ,-sep , 5*blockh, 0 , 0 , 0, r],
# [ blockl*5.5 - 0.03 , sep , 6*blockh, 0 , 0 , 0, 0],
# [ blockl*5.5 - 0.02 ,-sep , 6*blockh, 0 , 0 , 0, r]])
# continuous walking - first two blocks up
r = 1
flist = np.array([[blockl * -0.5 - 0.03, sep, 0, 0, 0, 0, 0],
[blockl * -0.5 + 0.03, -sep, 0, 0, 0, 0, r],
[blockl * 0.5 - 0.03, sep, blockh, 0, 0, 0, 0],
[blockl * 0.5 + 0.03, -sep, blockh, 0, 0, 0, r]])
contact_pts = self.footstepDriver.getContactPts()
contact_pts_mid = np.mean(
contact_pts, axis=0) # mid point on foot relative to foot frame
foot_to_sole = transformUtils.frameFromPositionAndRPY(
contact_pts_mid, [0, 0, 0]).GetLinearInverse()
flist_shape = flist.shape
self.goalSteps = []
for i in range(flist_shape[0]):
step_t = vtk.vtkTransform()
step_t.PostMultiply()
step_t.Concatenate(
transformUtils.frameFromPositionAndRPY(flist[i, 0:3],
flist[i, 3:6]))
step_t.Concatenate(foot_to_sole)
step_t.Concatenate(frame_pt_to_centerline)
step = lcmdrc.footstep_t()
step.pos = positionMessageFromFrame(step_t)
step.is_right_foot = flist[i, 6] # is_right_foot
step.params = self.footstepDriver.getDefaultStepParams()
# Visualization via triads
#vis.updateFrame(step_t, str(i), parent="navigation")
self.goalSteps.append(step)
startPose = self.jointController.q
request = self.footstepDriver.constructFootstepPlanRequest(startPose)
request.num_goal_steps = len(self.goalSteps)
request.goal_steps = self.goalSteps
lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty("Name")
if name in ("footstep widget", "footstep widget frame"):
om.removeFromObjectModel(om.findObjectByName("footstep widget"))
return True
match = re.match("^step (\d+)$", name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName("footstep widget")
if existingWidget:
previousStep = existingWidget.stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [
0.0, 0.0,
transformUtils.rollPitchYawFromTransform(footFrame)[2]
]
footFrame = transformUtils.frameFromPositionAndRPY(
footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh,
"footstep widget",
parent="planning",
alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame,
"footstep widget frame",
parent=footObj,
scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty("Color", obj.getProperty("Color"))
frameObj.setProperty("Edit", True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName("walking goal")
if walkGoal:
walkGoal.setProperty("Edit", False)
return True
class OptitrackVisualizer(object):
'''
Usage:
optitrackVis = OptitrackVisualizer('OPTITRACK_CHANNEL_NAME')
# You can enable visualization of edges between optitrack markers,
# but this visualization is slower. To enable:
optitrackVis.drawEdges = True
# By default the lcm update rate is throttled to 10 hz.
# To increase the update rate:
optitrackVis.subscriber.setSpeedLimit(100)
# Note, the constructor calls initSubscriber() automatically.
# To remove the lcm subscriber:
optitrackVis.removeSubscriber()
'''
defaultOptitrackToWorld = transformUtils.frameFromPositionAndRPY(
[0,0,0],[90,0,90])
def __init__(self, channel="OPTITRACK_FRAMES",
desc_channel="OPTITRACK_DATA_DESCRIPTIONS", name='Optitrack Visualier'):
self.name = name
self.channel = channel
self.desc_channel = desc_channel
self.subscriber = None
self.desc_subscriber = None
self.unitConversion = 0.001
self.data_descriptions = None
self.marker_sets = om.getOrCreateContainer(
"Marker Sets", parentObj=self.getRootFolder())
self.rigid_bodies = om.getOrCreateContainer(
"Rigid Bodies", parentObj=self.getRootFolder())
self.labeled_markers = om.getOrCreateContainer(
"Labeled Markers", parentObj=self.getRootFolder())
self.unlabeled_markers = om.getOrCreateContainer(
"Unlabeled Markers", parentObj=self.getRootFolder())
self.drawEdges = False
self.markerGeometry = None
self.optitrackToWorld = vtk.vtkTransform()
self.optitrackToWorld.SetMatrix(
self.defaultOptitrackToWorld.GetMatrix())
self.callbacks = callbacks.CallbackRegistry([
'RIGID_BODY_LIST_CHANGED',
])
def connectRigidBodyListChanged(self, func):
return self.callbacks.connect('RIGID_BODY_LIST_CHANGED', func)
def initSubscriber(self):
self.subscriber = lcmUtils.addSubscriber(
self.channel, optitrack_frame_t, self.onMessage)
self.subscriber.setSpeedLimit(10)
self.desc_subscriber = lcmUtils.addSubscriber(
self.desc_channel, optitrack_data_descriptions_t,
self.onDescMessage)
def removeSubscriber(self):
if self.subscriber is not None:
lcmUtils.removeSubscriber(self.subscriber)
self.subscriber = None
if self.desc_subscriber is not None:
lcmUtils.removeSubscriber(self.desc_subscriber)
self.desc_subscriber = None
def isEnabled(self):
return self.subscriber is not None
def setEnabled(self, enabled):
if enabled and not self.isEnabled():
self.initSubscriber()
elif not enabled and self.isEnabled():
self.removeSubscriber()
self.removeRootFolder()
def enable(self):
self.setEnabled(True)
def disable(self):
self.setEnabled(False)
def getRootFolder(self):
folder = om.getOrCreateContainer(self.channel)
return folder
def removeRootFolder(self):
om.removeFromObjectModel(self.getRootFolder())
def getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=8)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
def createMarkerObjects(self, marker_ids, modelFolder, modelName, modelColor):
geom = self.getMarkerGeometry()
visible = modelFolder.getProperty('Visible')
def makeMarker(i):
obj = vis.showPolyData(
shallowCopy(geom), modelName + ' marker %d' % i, color=modelColor, parent=modelFolder)
obj.setProperty('Visible', visible)
vis.addChildFrame(obj)
return obj
return [makeMarker(i) for i in marker_ids]
def setRobotBaseTransform(self, transform):
self.optitrackToWorld.Translate(transform.GetInverse().GetPosition())
# Move down a little to account for the fact that the markers
# aren't at the base of the robot.
self.optitrackToWorld.Translate(0, 0.025, 0)
# TODO(sam.creasey) handle rotation
def _updateMarkerCollection(self, prefix, folder, marker_ids,
positions, base_transform=None):
markers = folder.children()
if len(markers) != len(positions):
for obj in markers:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markers = self.createMarkerObjects(
marker_ids, folder, prefix, modelColor)
if len(markers):
modelColor = markers[0].getProperty('Color')
for i, pos in enumerate(positions):
marker_frame = transformUtils.transformFromPose(pos, (1, 0, 0, 0))
marker_frame = transformUtils.concatenateTransforms(
[marker_frame, self.optitrackToWorld])
if base_transform is not None:
marker_frame = transformUtils.concatenateTransforms(
[marker_frame, base_transform])
markers[i].getChildFrame().copyFrame(marker_frame)
# TODO(sam.creasey) we could try drawing edges here
def _handleMarkerSets(self, marker_sets):
# Get the list of existing marker sets so we can track any
# which disappear.
remaining_set_names = set(
[x.getProperty('Name') for x in self.marker_sets.children()])
for marker_set in marker_sets:
set_name = 'Marker set ' + marker_set.name
remaining_set_names.discard(set_name)
set_folder = om.getOrCreateContainer(
set_name, parentObj=self.marker_sets)
marker_ids = range(marker_set.num_markers)
self._updateMarkerCollection(
marker_set.name + '.', set_folder, marker_ids, marker_set.xyz)
for remaining_set in remaining_set_names:
obj = om.findObjectByName(remaining_set, self.marker_sets)
om.removeFromObjectModel(obj)
def _handleRigidBodies(self, rigid_bodies):
# Get the list of existing rigid bodies so we can track any
# which disappear.
remaining_body_names = set(
[x.getProperty('Name') for x in self.rigid_bodies.children()])
bodies_added = False
bodies_removed = False
for body in rigid_bodies:
body_name = 'Body ' + str(body.id)
for desc in self.data_descriptions.rigid_bodies:
if desc.id == body.id:
body_name = desc.name
if body_name in remaining_body_names:
body_obj = om.findObjectByName(
body_name, parent=self.rigid_bodies)
else:
bodies_added = True
# The use of a box here is arbitrary.
body_obj = affordanceitems.BoxAffordanceItem(
body_name, applogic.getCurrentRenderView())
om.addToObjectModel(body_obj, parentObj=self.rigid_bodies)
vis.addChildFrame(body_obj).setProperty('Deletable', False)
body_obj.setProperty('Surface Mode', 'Wireframe')
body_obj.setProperty('Color', [1,0,0])
remaining_body_names.discard(body_name)
x,y,z,w = body.quat
quat = (w,x,y,z)
objToOptitrack = transformUtils.transformFromPose(body.xyz, quat)
# Dimension our box based on a bounding across all of our
# markers.
all_xyz = body.marker_xyz + [body.xyz]
all_xyz = [(xyz[0] - body.xyz[0], xyz[1] - body.xyz[1], xyz[2] - body.xyz[2])
for xyz in all_xyz]
marker_transforms = [transformUtils.transformFromPose(xyz, (1, 0, 0, 0))
for xyz in all_xyz]
inv_transform = transformUtils.transformFromPose((0, 0, 0), (w, -x, -y, -z))
marker_transforms = [transformUtils.concatenateTransforms([t, inv_transform])
for t in marker_transforms]
all_xyz = [t.GetPosition() for t in marker_transforms]
(min_x, min_y, min_z) = (
min(xyz[0] for xyz in all_xyz),
min(xyz[1] for xyz in all_xyz),
min(xyz[2] for xyz in all_xyz))
(max_x, max_y, max_z) = (
max(xyz[0] for xyz in all_xyz),
max(xyz[1] for xyz in all_xyz),
max(xyz[2] for xyz in all_xyz))
dimensions = (
max(0.01, max_x - min_x),
max(0.01, max_y - min_y),
max(0.01, max_z - min_z))
#print "max, min", (max_x, max_y, max_z), (min_x, min_y, min_z)
body_obj.setProperty('Dimensions', dimensions)
objToWorld = transformUtils.concatenateTransforms([objToOptitrack, self.optitrackToWorld])
body_obj.getChildFrame().copyFrame(objToWorld)
folder = om.getOrCreateContainer('Models', parentObj=body_obj)
self._updateMarkerCollection(
body_name + '.', folder, body.marker_ids,
body.marker_xyz)#, base_transform=transform)
if len(remaining_body_names):
bodies_removed = True
for remaining_body in remaining_body_names:
obj = om.findObjectByName(remaining_body, self.rigid_bodies)
om.removeFromObjectModel(obj)
if bodies_added or bodies_removed:
self.callbacks.process(
'RIGID_BODY_LIST_CHANGED',
sorted([x.getProperty('Name')
for x in self.rigid_bodies.children()]))
def _handleLabeledMarkers(self, labeled_markers):
marker_ids = [x.id for x in labeled_markers]
marker_positions = [x.xyz for x in labeled_markers]
# We'll rename the items ourselves later.
self._updateMarkerCollection(
"dummy prefix", self.labeled_markers, marker_ids, marker_positions)
for i, marker in enumerate(self.labeled_markers.children()):
marker_name = 'Marker ' + str(marker_ids[i])
if marker.getProperty('Name') != marker_name:
marker.rename(marker_name)
def _handleUnlabeledMarkers(self, positions):
self._updateMarkerCollection(
"Unlabeled", self.unlabeled_markers, range(len(positions)),
positions)
def onMessage(self, msg):
self.lastMessage = msg
if self.data_descriptions is None:
return
self._handleMarkerSets(msg.marker_sets)
self._handleRigidBodies(msg.rigid_bodies)
#self._handleLabeledMarkers(msg.labeled_markers)
#self._handleUnlabeledMarkers(msg.other_markers)
def onDescMessage(self, msg):
self.data_descriptions = msg
def newWalkingGoal(self, displayPoint, view):
# put walking goal at robot's base
mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"]
footFrame = self.robotModel.getLinkFrame(mainLink)
if not footFrame:
print(
"ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in "
"the robot's URDF. Cannot place walking goal.".format(mainLink)
)
return
worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
groundOrigin = footFrame.GetPosition()
groundNormal = [0.0, 0.0, 1.0]
selectedGroundPoint = [0.0, 0.0, 0.0]
t = vtk.mutable(0.0)
vtk.vtkPlane.IntersectWithLine(
worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint
)
walkingTarget = transformUtils.frameFromPositionAndRPY(
selectedGroundPoint, np.array(footFrame.GetOrientation())
)
frameObj = vis.updateFrame(
walkingTarget,
self.robotName + " walking goal",
parent="planning",
scale=0.25,
)
frameObj.setProperty("Edit", True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName("HEIGHT_MAP_SCENE")
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1]
)
polyData = segmentation.thresholdPoints(
polyData, "distance_to_line", [0.0, 0.1]
)
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition())
)
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(
d.getPolyData(),
"walking goal terrain handle " + self.robotName,
parent=frameObj,
visible=True,
color=[1, 1, 0],
)
handle.actor.SetUserTransform(frameObj.transform)
placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == "Edit":
if propertySet.getProperty(propertyName):
placer.start()
else:
placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, "Edit")
frameObj.connectFrameModified(self.onWalkingGoalModified)
def toFrame(xyzrpy):
rpy = [math.degrees(rad) for rad in xyzrpy[3:]]
return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy)
def getCameraToKukaEndEffectorFrame():
return transformUtils.frameFromPositionAndRPY([0.1, 0, 0.0], [-90, -22.5, -90])
import numpy
if ikPlanner.pushToMatlab == True:
print "FAILURE - pushing requests to matlab"
exit()
qT = numpy.array([
0, 0, 0, 0, 0, 0, -6.310489698080346e-05, 0.34103086590766907,
3.8130277971504256e-05, 1.4273228645324707, 5.833456089021638e-05,
-0.4845042824745178, -3.8867587136337534e-05
])
q0 = numpy.array([
0., 0., 0., 0., 0., 0., 0., 0.78539816, 0., 1.57079633, 0., -0.78539816, 0.
])
goalFrame = transformUtils.frameFromPositionAndRPY(
[0.36932988056397303, -0.009998017176602909, 0.8891143571732633],
[-1.3262913021702864e-12, 89.99999979432002, -89.99963750134272])
constraintSet = ikPlanner.planEndEffectorGoal(q0,
'left',
goalFrame,
lockBase=True,
lockBack=True)
q = numpy.array(constraintSet.runIk()[0])
ret = constraintSet.runIkTraj()
if ((q - qT).__abs__() > 1e-3).any():
print "FAILURE - IK pose incorrect."
exit()
if ret.plan_info[0] != 0:
def computeAffordanceFrame(self):
position = self.properties.getProperty('Position')
rpy = self.properties.getProperty('Rotation')
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t.Concatenate(self.getGroundFrame())
return t
def setTagToWorld(pos, rpy):
global tagToWorld
tagToWorld = transformUtils.frameFromPositionAndRPY(pos, rpy)
def getDefaultCameraToWorld():
return transformUtils.frameFromPositionAndRPY([0,0,0], [-90,0,-90])
def updateTargetFrame(self):
q = self.robotModel.model.getJointPositions()
pos = q[:3]
rpy = np.degrees(q[3:6])
transform = transformUtils.frameFromPositionAndRPY(pos, rpy)
self.targetFrame.copyFrame(transform)
def transformOpticalFrameToBodyFrame(opticalFrame):
rpy = [-90, 0, -90]
opticalToBody = transformUtils.frameFromPositionAndRPY([0, 0, 0], rpy)
bodyFrame = transformUtils.concatenateTransforms(
[opticalToBody.GetLinearInverse(), opticalFrame])
return bodyFrame
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [
0.0, 0.0,
transformUtils.rollPitchYawFromTransform(footFrame)[2]
]
footFrame = transformUtils.frameFromPositionAndRPY(
footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh,
'footstep widget',
parent='planning',
alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame,
'footstep widget frame',
parent=footObj,
scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def initCameraFrustumVisualizer(depthScanner):
cameraObj = vis.showPolyData(vtk.vtkPolyData(),
'camera',
parent=depthScanner.parentFolder,
view=depthScanner.pointCloudView)
cameraFrame = vis.addChildFrame(cameraObj)
cameraFrame.setProperty('Scale', 50)
cameraObj.setProperty('Visible', False)
pointCloudToCamera = transformUtils.frameFromPositionAndRPY((
0,
0,
0,
), (-90, 90, 0)).GetLinearInverse()
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06],
center=[-0.02, 0.0, 0.0],
color=[1, 0.5, 0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0],
radius=0.023,
color=[1, 0.5, 0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(
cameraMesh,
getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def onCameraModified():
cameraToWorld = getCameraTransform(depthScanner.view.camera())
depthScanner.pointCloudObj.actor.SetUserTransform(
transformUtils.concatenateTransforms(
[pointCloudToCamera, cameraToWorld]))
cameraFrame.copyFrame(cameraToWorld)
def enableFrustum():
updateCameraMesh()
cameraObj.setProperty('Visible', True)
onCameraModified()
depthScanner._updateFunc = onCameraModified
applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True)
applogic.resetCamera(viewDirection=[1, 1, -0.4],
view=depthScanner.pointCloudView)
depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])
def disableFrustum():
cameraObj.setProperty('Visible', False)
depthScanner.pointCloudObj.actor.SetUserTransform(None)
depthScanner._updateFunc = None
applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView,
False)
applogic.resetCamera(viewDirection=[0, 0, -1],
view=depthScanner.pointCloudView)
depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])
return FieldContainer(updateCameraMesh=updateCameraMesh,
onCameraModified=onCameraModified,
enableFrustum=enableFrustum,
disableFrustum=disableFrustum)
