def getPelvisEstimate(self): p = robotStateModel.getLinkFrame('pelvis') lf = robotStateModel.getLinkFrame('l_foot') rf = robotStateModel.getLinkFrame('r_foot') pelvisLeft = self.getPelvisEstimateFromFoot(p, lf, self.lfootFrame) pelvisRight = self.getPelvisEstimateFromFoot(p, rf, self.rfootFrame) return transformUtils.frameInterpolate(pelvisLeft, pelvisRight, self.footBias)
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 planRoomSweep(self): self.initConstraintSet() faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 0) vis.showFrame(faceFrameDesired, 'frame 0', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 0) faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 1.0 / 3.0) vis.showFrame(faceFrameDesired, 'frame 1', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 1) faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 2.0 / 3.0) vis.showFrame(faceFrameDesired, 'frame 2', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 2) faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 3.0 / 3.0) vis.showFrame(faceFrameDesired, 'frame 3', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 3) #self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow self.planTrajectory()
def planRoomSweep(self): self.initConstraintSet() faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 0) vis.showFrame(faceFrameDesired, 'frame 0', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 0) faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 1.0/3.0) vis.showFrame(faceFrameDesired, 'frame 1', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 1) faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 2.0/3.0) vis.showFrame(faceFrameDesired, 'frame 2', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 2) faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 3.0/3.0) vis.showFrame(faceFrameDesired, 'frame 3', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 3) #self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow self.planTrajectory()
def interpolateSplineFrame(u): assert 0.0 <= u <= 1.0 pt = [0.0, 0.0, 0.0] self.splineWidget.GetRepresentation().GetParametricSpline().Evaluate([u,0.0,0.0], pt, range(9)) handleParameterization = self.computeHandleParameterization() if u >= handleParameterization[self.rotationEndIndex]: uu = 1.0 elif u <= handleParameterization[self.rotationStartIndex]: uu = 0.0 else: uu = (u - handleParameterization[self.rotationStartIndex]) / (handleParameterization[self.rotationEndIndex] - handleParameterization[self.rotationStartIndex]) #print 'rescaled u:', u, '-->', uu t = transformUtils.frameInterpolate(palmFrame, goalFrame, uu) t.PostMultiply() t.Translate(np.array(pt) - np.array(t.GetPosition())) return t