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)