def getNextDoubleSupportPose(self, lfootTransform, rfootTransform):
vis.updateFrame(lfootTransform, 'lfootTransform', visible=True, scale=0.2)
vis.updateFrame(rfootTransform, 'rfootTransform', visible=True, scale=0.2)
startPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
startPoseName = 'stride_start'
self.ikPlanner.addPose(startPose, startPoseName)
constraints = []
# lock everything except the feet, constrain the feet
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.append(self.ikPlanner.createMovingBackPostureConstraint())
constraints.append(self.ikPlanner.createMovingBasePostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
nullFrame = vtk.vtkTransform()
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.rightFootLink, rfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.leftFootLink, lfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ikplanner.ConstraintSet(self.ikPlanner, constraints, 'stride_end', startPoseName)
nextDoubleSupportPose, info = constraintSet.runIk()
return nextDoubleSupportPose
def getNextDoubleSupportPose(self, lfootTransform, rfootTransform):
vis.updateFrame(lfootTransform, 'lfootTransform', visible=True, scale=0.2)
vis.updateFrame(rfootTransform, 'rfootTransform', visible=True, scale=0.2)
startPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
startPoseName = 'stride_start'
self.ikPlanner.addPose(startPose, startPoseName)
constraints = []
# lock everything except the feet, constrain the feet
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.append(self.ikPlanner.createMovingBackPostureConstraint())
constraints.append(self.ikPlanner.createMovingBasePostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
nullFrame = vtk.vtkTransform()
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.rightFootLink, rfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.leftFootLink, lfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ikplanner.ConstraintSet(self.ikPlanner, constraints, 'stride_end', startPoseName)
nextDoubleSupportPose, info = constraintSet.runIk()
return nextDoubleSupportPose
def makeDebugRegions(self):
stepWidth = (15 + 3/8.0) * 0.0254
stepLength = (15 + 5/8.0) * 0.0254
stepHeight = (5 + 5/8.0) * 0.0254
stepPoints = np.array([
[-stepLength/2.0, -stepWidth/2.0, 0.0],
[-stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, -stepWidth/2.0, 0.0]
])
t = vtk.vtkTransform()
t.Translate(0.0, 0.0, 0.0)
t.RotateZ(4.5)
for i in xrange(len(stepPoints)):
stepPoints[i] = np.array(t.TransformPoint(stepPoints[i]))
stepOffset = np.array([stepLength, 0.0, stepHeight])
numSteps = 5
goalFrame = transformUtils.frameFromPositionAndRPY([0.4, 0.0, 0.1], [0,0,0])
vis.showFrame(goalFrame, 'goal frame', scale=0.2)
rpySeed = np.radians(goalFrame.GetOrientation())
for i in xrange(numSteps):
step = stepPoints + (i+1)*stepOffset
self.convertStepToSafeRegion(step, rpySeed)
self.footstepsPanel.onNewWalkingGoal(goalFrame)
def addEllipsoid(self,
center,
radii,
color=[1, 1, 1],
alpha=1.0,
resolution=24):
"""
Add an ellipsoid centered at [center] with x, y, and z principal axis radii given by
radii = [x_scale, y_scale, z_scale]
"""
sphere = vtk.vtkSphereSource()
sphere.SetCenter([0, 0, 0])
sphere.SetThetaResolution(resolution)
sphere.SetPhiResolution(resolution)
sphere.SetRadius(1.0)
sphere.Update()
transform = vtk.vtkTransform()
transform.Translate(center)
transform.Scale(radii)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(sphere.GetOutputPort())
transformFilter.Update()
self.addPolyData(transformFilter.GetOutput(), color)
def applyFrameTransform(x, y, z, yaw):
if lastEditedFrame is not None and lastEditedFrame.getProperty("Edit"):
t = vtk.vtkTransform()
t.Concatenate(lastEditedFrame.transform)
t.RotateZ(yaw)
t.Translate(x, y, z)
lastEditedFrame.copyFrame(t)
def onAprilTag(self, msg):
t = vtk.vtkTransform()
cameraview.imageManager.queue.getTransform('april_tag_car_beam',
'local', msg.utime, t)
self.originFrame.copyFrame(
transformUtils.concatenateTransforms(
[self.originToAprilTransform, t]))
def updateFrame(self):
norm = np.linalg.norm(np.array([self.thetadot, self.phidot, self.yawdot, self.Xdot, self.Ydot, self.Zdot]))
dt = 0.01 #self.timer.elapsed
if self.baseFrame is not None:
cameraFocus = np.asarray(self.camera.GetFocalPoint())
cameraInterp = cameraFocus + (self.cameraTarget - cameraFocus)*0.02
self.camera.SetFocalPoint(cameraInterp)
if self.cameraMode is not True:
if self.baseFrame is not None and norm > 0.1:
t = vtk.vtkTransform()
t.Concatenate(self.baseFrame.transform)
t.RotateZ(-self.thetadot * dt * self.speedMultiplier)
t.RotateX(self.phidot * dt * self.speedMultiplier)
t.RotateY(self.yawdot * dt * self.speedMultiplier)
t.Translate(self.Xdot * dt * self.speedMultiplier, self.Ydot * dt * self.speedMultiplier, self.Zdot * dt * self.speedMultiplier)
self.baseFrame.copyFrame(t)
else:
self.camera.Elevation(self.Zdot * self.speedMultiplier)
self.camera.Azimuth(self.Xdot * self.speedMultiplier)
self.camera.Zoom(1.0 + self.phidot/1000.0)
self.view.render()
def updateFrame(self):
norm = np.linalg.norm(
np.array([
self.thetadot, self.phidot, self.yawdot, self.Xdot, self.Ydot,
self.Zdot
]))
dt = 0.01 #self.timer.elapsed
if self.baseFrame is not None:
cameraFocus = np.asarray(self.camera.GetFocalPoint())
cameraInterp = cameraFocus + (self.cameraTarget -
cameraFocus) * 0.02
self.camera.SetFocalPoint(cameraInterp)
if self.cameraMode is not True:
if self.baseFrame is not None and norm > 0.1:
t = vtk.vtkTransform()
t.Concatenate(self.baseFrame.transform)
t.RotateZ(-self.thetadot * dt * self.speedMultiplier)
t.RotateX(self.phidot * dt * self.speedMultiplier)
t.RotateY(self.yawdot * dt * self.speedMultiplier)
t.Translate(self.Xdot * dt * self.speedMultiplier,
self.Ydot * dt * self.speedMultiplier,
self.Zdot * dt * self.speedMultiplier)
self.baseFrame.copyFrame(t)
else:
self.camera.Elevation(self.Zdot * self.speedMultiplier)
self.camera.Azimuth(self.Xdot * self.speedMultiplier)
self.camera.Zoom(1.0 + self.phidot / 1000.0)
self.view.render()
def makeDebugRegions(self):
stepWidth = (15 + 3/8.0) * 0.0254
stepLength = (15 + 5/8.0) * 0.0254
stepHeight = (5 + 5/8.0) * 0.0254
stepPoints = np.array([
[-stepLength/2.0, -stepWidth/2.0, 0.0],
[-stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, -stepWidth/2.0, 0.0]
])
t = vtk.vtkTransform()
t.Translate(0.0, 0.0, 0.0)
t.RotateZ(4.5)
for i in xrange(len(stepPoints)):
stepPoints[i] = np.array(t.TransformPoint(stepPoints[i]))
stepOffset = np.array([stepLength, 0.0, stepHeight])
numSteps = 5
goalFrame = transformUtils.frameFromPositionAndRPY([0.4, 0.0, 0.1], [0,0,0])
vis.showFrame(goalFrame, 'goal frame', scale=0.2)
rpySeed = np.radians(goalFrame.GetOrientation())
for i in xrange(numSteps):
step = stepPoints + (i+1)*stepOffset
self.convertStepToSafeRegion(step, rpySeed)
self.footstepsPanel.onNewWalkingGoal(goalFrame)
def applyFrameTransform(x, y, z, yaw):
if lastEditedFrame is not None and lastEditedFrame.getProperty('Edit'):
t = vtk.vtkTransform()
t.Concatenate(lastEditedFrame.transform)
t.RotateZ(yaw)
t.Translate(x, y, z)
lastEditedFrame.copyFrame(t)
def getTransformFromNumpy(mat):
'''
Given a numpy 4x4 array, return a vtkTransform.
'''
assert mat.shape == (4,4)
t = vtk.vtkTransform()
t.SetMatrix(mat.flatten())
return t
def createLeftFootPoseConstraint(self,
targetFrame,
tspan=[-np.inf, np.inf]):
positionConstraint, orientationConstraint = self.robotSystem.ikPlanner.createPositionOrientationConstraint(
'l_foot', targetFrame, vtk.vtkTransform())
positionConstraint.tspan = tspan
orientationConstraint.tspan = tspan
return positionConstraint, orientationConstraint
def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):
print 'planning with safe regions. %d blocks.' % len(blocks)
folder = om.getOrCreateContainer('Safe terrain regions')
om.removeFromObjectModel(folder)
footsteps = []
for i, block in enumerate(blocks):
corners = block.getCorners()
rpy = np.radians(block.cornerTransform.GetOrientation())
#rpy = [0.0, 0.0, 0.0]
self.convertStepToSafeRegion(corners, rpy)
lastBlock = blocks[-1]
goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
goalOffset = vtk.vtkTransform()
goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
goalFrame.PreMultiply()
goalFrame.Concatenate(goalOffset)
goalPosition = np.array(goalFrame.GetPosition())
if len(blocks) > 1:
goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))
vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)
request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)
assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
if standingFootName == self.ikPlanner.rightFootLink:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
else:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT
request.params.leading_foot = leadingFoot
request.params.max_forward_step = 0.5
request.params.nom_forward_step = 0.12
request.params.nom_step_width = 0.22
request.params.max_num_steps = 8 #2*len(blocks)
plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)
if not plan:
return []
#print 'received footstep plan with %d steps.' % len(plan.footsteps)
footsteps = []
for i, footstep in enumerate(plan.footsteps):
footstepTransform = self.transformFromFootstep(footstep)
footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))
return footsteps[2:]
def computeFootstepPlanSafeRegions(self, blocks, robotPose, standingFootName):
print 'planning with safe regions. %d blocks.' % len(blocks)
folder = om.getOrCreateContainer('Safe terrain regions')
om.removeFromObjectModel(folder)
footsteps = []
for i, block in enumerate(blocks):
corners = block.getCorners()
rpy = np.radians(block.cornerTransform.GetOrientation())
#rpy = [0.0, 0.0, 0.0]
self.convertStepToSafeRegion(corners, rpy)
lastBlock = blocks[-1]
goalFrame = transformUtils.copyFrame(lastBlock.cornerTransform)
goalOffset = vtk.vtkTransform()
goalOffset.Translate(0.3, lastBlock.rectWidth/2.0, 0.0)
goalFrame.PreMultiply()
goalFrame.Concatenate(goalOffset)
goalPosition = np.array(goalFrame.GetPosition())
if len(blocks) > 1:
goalFrame = transformUtils.copyFrame(blocks[-2].cornerTransform)
goalFrame.Translate(goalPosition - np.array(goalFrame.GetPosition()))
vis.updateFrame(goalFrame, 'footstep plan goal', scale=0.2)
request = self.footstepsPanel.driver.constructFootstepPlanRequest(robotPose, goalFrame)
assert standingFootName in (self.ikPlanner.leftFootLink, self.ikPlanner.rightFootLink)
if standingFootName == self.ikPlanner.rightFootLink:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_RIGHT
else:
leadingFoot = lcmdrc.footstep_plan_params_t.LEAD_LEFT
request.params.leading_foot = leadingFoot
request.params.max_forward_step = 0.5
request.params.nom_forward_step = 0.12
request.params.nom_step_width = 0.22
request.params.max_num_steps = 8 #2*len(blocks)
plan = self.footstepsPanel.driver.sendFootstepPlanRequest(request, waitForResponse=True)
if not plan:
return []
#print 'received footstep plan with %d steps.' % len(plan.footsteps)
footsteps = []
for i, footstep in enumerate(plan.footsteps):
footstepTransform = self.transformFromFootstep(footstep)
footsteps.append(Footstep(footstepTransform, footstep.is_right_foot))
return footsteps[2:]
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(director.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def concatenateTransforms(transformList):
'''
Given a list of vtkTransform objects, returns a new vtkTransform
which is a concatenation of the whole list using vtk post multiply.
See documentation for vtkTransform::PostMultiply.
'''
result = vtk.vtkTransform()
result.PostMultiply()
for t in transformList:
result.Concatenate(t)
return result
def concatenateTransforms(transformList):
'''
Given a list of vtkTransform objects, returns a new vtkTransform
which is a concatenation of the whole list using vtk post multiply.
See documentation for vtkTransform::PostMultiply.
'''
result = vtk.vtkTransform()
result.PostMultiply()
for t in transformList:
result.Concatenate(t)
return result
def getTransformFromNumpy(mat):
'''
Given a numpy 4x4 array, return a vtkTransform.
'''
m = vtk.vtkMatrix4x4()
for r in xrange(4):
for c in xrange(4):
m.SetElement(r, c, mat[r][c])
t = vtk.vtkTransform()
t.SetMatrix(m)
return t
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 getTransformFromNumpy(mat):
'''
Given a numpy 4x4 array, return a vtkTransform.
'''
m = vtk.vtkMatrix4x4()
for r in xrange(4):
for c in xrange(4):
m.SetElement(r, c, mat[r][c])
t = vtk.vtkTransform()
t.SetMatrix(m)
return t
def onStartMappingButton(self):
msg = map_command_t()
msg.timestamp = getUtime()
msg.command = 0
lcmUtils.publish('KINECT_MAP_COMMAND', msg)
utime = self.queue.getCurrentImageTime('KINECT_RGB')
self.cameraToLocalInit = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, self.cameraToLocalInit)
vis.updateFrame(self.cameraToLocalInit, 'initial cam' )
print "starting mapping", utime
print self.cameraToLocalInit.GetPosition()
print self.cameraToLocalInit.GetOrientation()
def transformFromPose(position, quaternion):
'''
Returns a vtkTransform
'''
rotationMatrix = np.zeros((3, 3))
vtk.vtkMath.QuaternionToMatrix3x3(quaternion, rotationMatrix)
mat = np.eye(4)
mat[:3, :3] = rotationMatrix
mat[:3, 3] = position
t = vtk.vtkTransform()
t.SetMatrix(mat.flatten())
return t
def getTransformFromAxes(xaxis, yaxis, zaxis):
t = vtk.vtkTransform()
m = vtk.vtkMatrix4x4()
axes = np.array([xaxis, yaxis, zaxis]).transpose().copy()
vtk.vtkMath.Orthogonalize3x3(axes, axes)
for r in xrange(3):
for c in xrange(3):
m.SetElement(r, c, axes[r][c])
t.SetMatrix(m)
return t
def frameFromPositionAndRPY(position, rpy):
'''
rpy specified in degrees
'''
rpy = [math.radians(deg) for deg in rpy]
angle, axis = botpy.roll_pitch_yaw_to_angle_axis(rpy)
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateWXYZ(math.degrees(angle), axis)
t.Translate(position)
return t
def getTransformFromAxes(xaxis, yaxis, zaxis):
t = vtk.vtkTransform()
m = vtk.vtkMatrix4x4()
axes = np.array([xaxis, yaxis, zaxis]).transpose().copy()
vtk.vtkMath.Orthogonalize3x3(axes, axes)
for r in xrange(3):
for c in xrange(3):
m.SetElement(r, c, axes[r][c])
t.SetMatrix(m)
return t
def transformFromPose(position, quaternion):
'''
Returns a vtkTransform
'''
rotationMatrix = np.zeros((3,3))
vtk.vtkMath.QuaternionToMatrix3x3(quaternion, rotationMatrix)
mat = np.eye(4)
mat[:3,:3] = rotationMatrix
mat[:3,3] = position
t = vtk.vtkTransform()
t.SetMatrix(mat.flatten())
return t
def frameFromPositionAndRPY(position, rpy):
'''
rpy specified in degrees
'''
rpy = [math.radians(deg) for deg in rpy]
angle, axis = botpy.roll_pitch_yaw_to_angle_axis(rpy)
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateWXYZ(math.degrees(angle), axis)
t.Translate(position)
return t
def addTorus(self, radius, thickness, resolution=30):
q = vtk.vtkSuperquadricSource()
q.SetToroidal(1)
q.SetSize(radius)
q.SetThetaResolution(resolution)
# thickness doesnt seem to match to Eucliean units. 0 is none. 1 is full. .1 is a good valve
q.SetThickness(thickness)
q.Update()
# rotate Torus so that the hole axis (internally y), is set to be z, which we use for valves
transform = vtk.vtkTransform()
transform.RotateWXYZ(90,1,0,0)
transformFilter=vtk.vtkTransformPolyDataFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(q.GetOutputPort())
transformFilter.Update()
self.addPolyData(transformFilter.GetOutput())
def addEllipsoid(self, center, radii, color=[1,1,1], alpha=1.0, resolution=24):
"""
Add an ellipsoid centered at [center] with x, y, and z principal axis radii given by
radii = [x_scale, y_scale, z_scale]
"""
sphere = vtk.vtkSphereSource()
sphere.SetCenter([0,0,0])
sphere.SetThetaResolution(resolution)
sphere.SetPhiResolution(resolution)
sphere.SetRadius(1.0)
sphere.Update()
transform = vtk.vtkTransform()
transform.Translate(center)
transform.Scale(radii)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetTransform(transform)
transformFilter.SetInputConnection(sphere.GetOutputPort())
transformFilter.Update()
self.addPolyData(transformFilter.GetOutput())
def copyFrame(transform):
t = vtk.vtkTransform()
t.PostMultiply()
t.SetMatrix(transform.GetMatrix())
return t
def __init__(self):
self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag)
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
desc = dict(classname='MeshAffordanceItem', Name='polaris',
Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
self.originFrame = self.pointcloudAffordance.getChildFrame()
self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508 , -0.00282131, -0.01000079]),
np.array([ 9.99997498e-01, -2.10472556e-03, -1.33815696e-04, 7.46246794e-04])) # offset for . . . who knows why
# t = transformUtils.transformFromPose(np.array([ 0.14376024, 0.95920689, 0.36655712]), np.array([ 0.28745842, 0.90741428, -0.28822068, 0.10438304]))
t = transformUtils.transformFromPose(np.array([ 0.10873244, 0.93162364, 0.40509084]),
np.array([ 0.32997378, 0.88498408, -0.31780588, 0.08318602]))
self.leftFootEgressStartFrame = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.265, 0.874, 0.274]),
np.array([ 0.35290731, 0.93443693, -0.04181263, 0.02314636]))
self.leftFootEgressMidFrame = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.54339115, 0.89436275, 0.26681047]),
np.array([ 0.34635985, 0.93680077, -0.04152008, 0.02674412]))
self.leftFootEgressOutsideFrame = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance)
# pose = [np.array([-0.78962299, 0.44284877, -0.29539116]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] #old location
# pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] # updated location
pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.53047159, 0.46554963, -0.48086192, 0.52022615])] # update orientation
desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose,
Color=[1, 0, 0], Radius=float(0.18), Segments=20)
self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
pose = [np.array([-0.05907324, 0.80460545, 0.45439687]), np.array([ 0.14288327, 0.685944 , -0.703969 , 0.11615873])]
desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0])
self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
# t = transformUtils.transformFromPose(np.array([ 0.04045136, 0.96565326, 0.25810111]),
# np.array([ 0.26484648, 0.88360091, -0.37065556, -0.10825996]))
# t = transformUtils.transformFromPose(np.array([ -4.34908919e-04, 9.24901627e-01, 2.65614116e-01]),
# np.array([ 0.25022251, 0.913271 , -0.32136359, -0.00708626]))
t = transformUtils.transformFromPose(np.array([ 0.0384547 , 0.89273742, 0.24140762]),
np.array([ 0.26331831, 0.915796 , -0.28055337, 0.11519963]))
self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763, 0.34897024]),
np.array([ 0.03879584, 0.98950919, 0.03820214, 0.13381721]))
self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance)
# t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
# np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
# t = transformUtils.transformFromPose(np.array([-0.14940375, 0.90347275, 0.23812658]),
# np.array([ 0.27150909, 0.91398724, -0.28877386, 0.0867167 ]))
# t = transformUtils.transformFromPose(np.array([-0.17331227, 0.87879312, 0.25645152]),
# np.array([ 0.26344489, 0.91567196, -0.28089824, 0.11505581]))
# self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.13194951, 0.89871423, 0.24956246]),
np.array([ 0.21589082, 0.91727326, -0.30088849, 0.14651633]))
self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.17712239, 0.87619935, 0.27001509]),
np.array([ 0.33484372, 0.88280787, -0.31946488, 0.08044963]))
self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998, 0.01375837]),
np.array([ 6.10521653e-03, 4.18621358e-04, 4.65520611e-01,
8.85015882e-01]))
self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
def createLeftFootPoseConstraint(self, targetFrame, tspan=[-np.inf,np.inf]):
positionConstraint, orientationConstraint = self.robotSystem.ikPlanner.createPositionOrientationConstraint('l_foot', targetFrame, vtk.vtkTransform())
positionConstraint.tspan = tspan
orientationConstraint.tspan = tspan
return positionConstraint, orientationConstraint
def onAprilTag(self, msg):
t = vtk.vtkTransform()
cameraview.imageManager.queue.getTransform('april_tag_car_beam', 'local', msg.utime, t)
self.originFrame.copyFrame(transformUtils.concatenateTransforms([self.originToAprilTransform, t]))
def copyFrame(transform):
t = vtk.vtkTransform()
t.PostMultiply()
t.SetMatrix(transform.GetMatrix())
return t
def getThumbToPalmFrame(self):
return vtk.vtkTransform()
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 = transformUtils.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 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 = transformUtils.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 getThumbToPalmFrame(self):
return vtk.vtkTransform()
