def computeGraspPlan(self,
targetFrame,
graspToHandFrame,
inLine=False,
ikParameters=None):
startPose = self.getPlanningStartPose()
endPose, constraintSet = self.computeGraspPose(startPose, targetFrame)
if ikParameters:
constraintSet.ikParameters = ikParameters
constraintSet.ikParameters.usePointwise = False
if inLine:
handLinkName = self.ikPlanner.getHandLink(self.graspingHand)
graspToHand = graspToHandFrame
handToWorld1 = self.ikPlanner.getLinkFrameAtPose(
handLinkName, startPose)
handToWorld2 = self.ikPlanner.getLinkFrameAtPose(
handLinkName, endPose)
handToWorld1 = transformUtils.concatenateTransforms(
[graspToHand, handToWorld1])
handToWorld2 = transformUtils.concatenateTransforms(
[graspToHand, handToWorld2])
motionVector = np.array(handToWorld2.GetPosition()) - np.array(
handToWorld1.GetPosition())
motionTargetFrame = transformUtils.getTransformFromOriginAndNormal(
np.array(handToWorld2.GetPosition()), motionVector)
#vis.updateFrame(motionTargetFrame, 'motion target frame', scale=0.1)
#d = DebugData()
#d.addLine(np.array(handToWorld2.GetPosition()), np.array(handToWorld2.GetPosition()) - motionVector)
#vis.updatePolyData(d.getPolyData(), 'motion vector', visible=False)
p = self.ikPlanner.createLinePositionConstraint(
handLinkName,
graspToHand,
motionTargetFrame,
lineAxis=2,
bounds=[-np.linalg.norm(motionVector), 0.001],
positionTolerance=0.001)
p.tspan = np.linspace(0, 1, 5)
constraintSet.constraints.append(p)
newPlan = constraintSet.runIkTraj()
else:
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
return newPlan
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 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 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 getCameraToLocal(self):
'''
Returns cameraToLocal. cameraToHead is pulled from bot frames while
headToLocal is pulled from the robot model forward kinematics.
'''
headToLocal = self.robotModel.getLinkFrame( self.headLink )
cameraToHead = vtk.vtkTransform()
# TODO: 'head' should match self.headLink
self.imageManager.queue.getTransform(self.cameraName, 'head', 0, cameraToHead)
return transformUtils.concatenateTransforms([cameraToHead, headToLocal])
def computeGraspPlan(self, targetFrame, graspToHandFrame, inLine=False, ikParameters=None):
startPose = self.getPlanningStartPose()
endPose, constraintSet = self.computeGraspPose(startPose, targetFrame)
if ikParameters:
constraintSet.ikParameters = ikParameters
constraintSet.ikParameters.usePointwise = False
if inLine:
handLinkName = self.ikPlanner.getHandLink(self.graspingHand)
graspToHand = graspToHandFrame
handToWorld1 = self.ikPlanner.getLinkFrameAtPose(handLinkName, startPose)
handToWorld2 = self.ikPlanner.getLinkFrameAtPose(handLinkName, endPose)
handToWorld1 = transformUtils.concatenateTransforms([graspToHand, handToWorld1])
handToWorld2 = transformUtils.concatenateTransforms([graspToHand, handToWorld2])
motionVector = np.array(handToWorld2.GetPosition()) - np.array(handToWorld1.GetPosition())
motionTargetFrame = transformUtils.getTransformFromOriginAndNormal(np.array(handToWorld2.GetPosition()), motionVector)
#vis.updateFrame(motionTargetFrame, 'motion target frame', scale=0.1)
#d = DebugData()
#d.addLine(np.array(handToWorld2.GetPosition()), np.array(handToWorld2.GetPosition()) - motionVector)
#vis.updatePolyData(d.getPolyData(), 'motion vector', visible=False)
p = self.ikPlanner.createLinePositionConstraint(handLinkName, graspToHand, motionTargetFrame,
lineAxis=2, bounds=[-np.linalg.norm(motionVector), 0.001], positionTolerance=0.001)
p.tspan = np.linspace(0, 1, 5)
constraintSet.constraints.append(p)
newPlan = constraintSet.runIkTraj()
else:
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
return newPlan
def getCameraToLocal(self):
'''
Returns cameraToLocal. cameraToHead is pulled from bot frames while
headToLocal is pulled from the robot model forward kinematics.
'''
headToLocal = self.robotModel.getLinkFrame(self.headLink)
cameraToHead = vtk.vtkTransform()
# TODO: 'head' should match self.headLink
self.imageManager.queue.getTransform(self.cameraName, 'head', 0,
cameraToHead)
return transformUtils.concatenateTransforms(
[cameraToHead, headToLocal])
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 onFootstepStatus(self, msg):
#print "got message"
import ipab
#print msg.actual_foot_position_in_world[0], msg.actual_foot_position_in_world[1], msg.actual_foot_position_in_world[2]
#print msg.actual_foot_orientation_in_world[0], msg.actual_foot_orientation_in_world[1], msg.actual_foot_orientation_in_world[2], msg.actual_foot_orientation_in_world[3]
#print msg.LEFT
#print msg.RIGHT
x = msg.actual_foot_position_in_world[0]
y = msg.actual_foot_position_in_world[1]
z = msg.actual_foot_position_in_world[2]
q1 = msg.actual_foot_orientation_in_world[0]
q2 = msg.actual_foot_orientation_in_world[1]
q3 = msg.actual_foot_orientation_in_world[2]
q4 = msg.actual_foot_orientation_in_world[3]
if msg.status == 1:
tf_footStatus = transformUtils.transformFromPose([x,y,z], [q1,q2,q3,q4])
self.transforms_series[:] = []
self.transforms_series.append(tf_footStatus)
self.transforms_series.append(self.tf_robotStatus.GetInverse())
tf_foot_robot = transformUtils.concatenateTransforms(self.transforms_series)
self.footstep_index = self.footstep_index + 1
# Check what foot is in contact
#print 'self.footstep_index'
#print self.footstep_index
[robot_pos, robot_ori] = transformUtils.poseFromTransform(self.tf_robotStatus)
[current_pos, current_ori] = transformUtils.poseFromTransform(tf_foot_robot)
if (current_pos[1] > 0):
current_left = True
else:
current_left = False
#print 'current:'
#print (current_pos)
#print 'robot:'
#print (robot_pos)
#print 'Left?'
#print current_left
# I want to take the first status for the LEFT foot
if self.new_status and current_left:
# left foot in contact (reached left single support)
self.footStatus.append(Footstep(tf_footStatus, 0))
self.new_first_double_supp = True
self.new_status = False
# right foot expected pose (from planning)
if self.footstep_index+1 < len(self.planned_footsteps):
if (self.planned_footsteps[self.footstep_index+1].is_right_foot):
self.footStatus.append(self.planned_footsteps[self.footstep_index+1])
else:
self.footStatus.append(self.planned_footsteps[self.footstep_index])
else:
self.footStatus.append(self.footStatus_right[len(self.footStatus_right)-1])
elif not current_left:
# right foot in contact
self.footStatus_right.append(Footstep(tf_footStatus, 1))
else:
self.new_status = True
if (len(self.footStatus) > 0 and self.new_first_double_supp):
t1 = self.footStatus[len(self.footStatus)-2].transform
t2 = self.footStatus[len(self.footStatus)-1].transform
#print 't1 and t2:'
#print (transformUtils.poseFromTransform(t1))
#print (transformUtils.poseFromTransform(t2))
#print 'list lenght:'
#print (len(self.footStatus))
self.new_first_double_supp = False
if self.footStatus[len(self.footStatus)-2].is_right_foot:
t1, t2 = t2, t1
pose = self.getNextDoubleSupportPose(t1, t2)
self.displayExpectedPose(pose)
standingFootName = self.ikPlanner.rightFootLink if self.footStatus[len(self.footStatus)-2].is_right_foot else self.ikPlanner.leftFootLink
self.makeReplanRequest(standingFootName, removeFirstLeftStep = False, nextDoubleSupportPose=pose)
