def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False): assert leadingFoot in ('left', 'right') isRightFootOffset = 0 if leadingFoot == 'left' else 1 footOriginToSole = -np.mean(FootstepsDriver.getContactPts(), axis=0) stepMessages = [] for i, stepFrame in enumerate(stepFrames): t = transformUtils.copyFrame(stepFrame) t.PreMultiply() t.Translate(footOriginToSole) step = lcmdrc.footstep_t() step.pos = transformUtils.positionMessageFromFrame(t) step.is_right_foot = (i + isRightFootOffset) % 2 step.params = self.footstepsDriver.getDefaultStepParams() step.fixed_x = True step.fixed_y = True step.fixed_z = True step.fixed_roll = True step.fixed_pitch = True step.fixed_yaw = True if snapToTerrain: step.fixed_z = False step.fixed_roll = False step.fixed_pitch = False stepMessages.append(step) return stepMessages
def transformPlanToBDIFrame(self, plan): if (self.pose_bdi is None): # print "haven't received POSE_BDI" return # TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame # instead of using FK here t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation) t_bodybdi.PostMultiply() current_pose = self.jointController.q t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , botpy.roll_pitch_yaw_to_quat(current_pose[3:6]) ) t_bodymain.PostMultiply() # iterate and transform self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy for i, footstep in enumerate(self.bdi_plan.footsteps): step = footstep.pos t_step = transformUtils.frameFromPositionMessage(step) t_body_to_step = vtk.vtkTransform() t_body_to_step.DeepCopy(t_step) t_body_to_step.PostMultiply() t_body_to_step.Concatenate(t_bodymain.GetLinearInverse()) t_stepbdi = vtk.vtkTransform() t_stepbdi.DeepCopy(t_body_to_step) t_stepbdi.PostMultiply() t_stepbdi.Concatenate(t_bodybdi) footstep.pos = transformUtils.positionMessageFromFrame(t_stepbdi) if (self.showBDIPlan is True): self.drawBDIFootstepPlan()
def transformPlanToBDIFrame(self, plan): if (self.pose_bdi is None): # print "haven't received POSE_BDI" return # TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame # instead of using FK here t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation) t_bodybdi.PostMultiply() current_pose = self.jointController.q t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , transformUtils.rollPitchYawToQuaternion(current_pose[3:6]) ) t_bodymain.PostMultiply() # iterate and transform self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy for i, footstep in enumerate(self.bdi_plan.footsteps): step = footstep.pos t_step = transformUtils.frameFromPositionMessage(step) t_body_to_step = vtk.vtkTransform() t_body_to_step.DeepCopy(t_step) t_body_to_step.PostMultiply() t_body_to_step.Concatenate(t_bodymain.GetLinearInverse()) t_stepbdi = vtk.vtkTransform() t_stepbdi.DeepCopy(t_body_to_step) t_stepbdi.PostMultiply() t_stepbdi.Concatenate(t_bodybdi) footstep.pos = transformUtils.positionMessageFromFrame(t_stepbdi) if (self.showBDIPlan is True): self.drawBDIFootstepPlan()
def requestIRISRegion(self, tform, uid, bounding_box_width=2): start = np.asarray(tform.GetPosition()) A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width) msg = lcmdrc.iris_region_request_t() msg.utime = getUtime() msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q) msg.map_mode = self.map_mode_map[self.params.properties.map_mode] msg.num_seed_poses = 1 msg.seed_poses = [transformUtils.positionMessageFromFrame(tform)] msg.region_id = [uid] msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)] lcmUtils.publish(self.request_channel, msg)
def requestIRISRegion(self, tform, uid, bounding_box_width=2): start = np.asarray(tform.GetPosition()) A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width) msg = lcmdrc.iris_region_request_t() msg.utime = getUtime() msg.initial_state = robotstate.drakePoseToRobotState( self.jointController.q) msg.map_mode = self.map_mode_map[self.params.properties.map_mode] msg.num_seed_poses = 1 msg.seed_poses = [transformUtils.positionMessageFromFrame(tform)] msg.region_id = [uid] msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)] lcmUtils.publish(self.request_channel, msg)
def constructFootstepPlanRequest(self, pose, goalFrame=None): msg = lcmdrc.footstep_plan_request_t() msg.utime = getUtime() state_msg = robotstate.drakePoseToRobotState(pose) msg.initial_state = state_msg if goalFrame is None: goalFrame = vtk.vtkTransform() msg.goal_pos = transformUtils.positionMessageFromFrame(goalFrame) msg = self.applyParams(msg) msg = self.applySafeRegions(msg) return msg
def createGoalSteps(self, model, pose): distanceForward = 1.0 fr = model.getLinkFrame(_leftFootLink) fl = model.getLinkFrame(_rightFootLink) pelvisT = model.getLinkFrame(_pelvisLink) xaxis = [1.0, 0.0, 0.0] pelvisT.TransformVector(xaxis, xaxis) xaxis = np.array(xaxis) zaxis = np.array([0.0, 0.0, 1.0]) yaxis = np.cross(zaxis, xaxis) xaxis = np.cross(yaxis, zaxis) numGoalSteps = 3 is_right_foot = True self.goalSteps = [] for i in range(numGoalSteps): t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() if is_right_foot: t.Translate(fr.GetPosition()) else: t.Translate(fl.GetPosition()) t.Translate(xaxis*distanceForward) distanceForward += 0.15 is_right_foot = not is_right_foot step = lcmdrc.footstep_t() step.pos = transformUtils.positionMessageFromFrame(t) step.is_right_foot = is_right_foot step.params = self.getDefaultStepParams() self.goalSteps.append(step) request = self.constructFootstepPlanRequest(pose) request.num_goal_steps = len(self.goalSteps) request.goal_steps = self.goalSteps self.sendFootstepPlanRequest(request)
def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False): assert leadingFoot in ('left', 'right') isRightFootOffset = 0 if leadingFoot == 'left' else 1 leftPoints, rightPoints = FootstepsDriver.getContactPts() # note, assumes symmetrical feet. the loop below should be # updated to alternate between left/right contact point sets footOriginToSole = -np.mean(leftPoints, axis=0) stepMessages = [] for i, stepFrame in enumerate(stepFrames): t = transformUtils.copyFrame(stepFrame) t.PreMultiply() t.Translate(footOriginToSole) step = lcmdrc.footstep_t() step.pos = transformUtils.positionMessageFromFrame(t) step.is_right_foot = (i + isRightFootOffset) % 2 step.params = self.footstepsDriver.getDefaultStepParams() step.fixed_x = True step.fixed_y = True step.fixed_z = True step.fixed_roll = True step.fixed_pitch = True step.fixed_yaw = True if snapToTerrain: step.fixed_z = False step.fixed_roll = False step.fixed_pitch = False stepMessages.append(step) return stepMessages
def sendPlanningRequest(self, footsteps, nextDoubleSupportPose): goalSteps = [] #for i in range(flist_shape[0]): for i, footstep in enumerate(footsteps): #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_t = footstep.transform step = lcmdrc.footstep_t() step.pos = transformUtils.positionMessageFromFrame(step_t) step.is_right_foot = footstep.is_right_foot # flist[i,6] # is_right_foot step.params = self.footstepsPanel.driver.getDefaultStepParams() # Visualization via triads #vis.updateFrame(step_t, str(i), parent="navigation") goalSteps.append(step) #nextDoubleSupportPose = self.robotStateJointController.q request = self.footstepsPanel.driver.constructFootstepPlanRequest(nextDoubleSupportPose) request.num_goal_steps = len(goalSteps) request.goal_steps = goalSteps # force correct planning parameters: request.params.leading_foot = goalSteps[0].is_right_foot request.params.planning_mode = 1 request.params.nom_forward_step = 0.38 request.params.map_mode = 1 # 2 footplane, 0 h+n, 1 h+zup, 3 hori request.params.max_num_steps = len(goalSteps) request.params.min_num_steps = len(goalSteps) lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
def to_iris_region_t(self): msg = lcmdrc.iris_region_t() msg.lin_con = self.to_lin_con_t() msg.seed_pose = transformUtils.positionMessageFromFrame(self.tform) return msg
def onStepModified(self, ndx, frameObj): self.lastFootstepPlan.footsteps[ndx+2].pos = transformUtils.positionMessageFromFrame(frameObj.transform) self.lastFootstepPlan.footsteps[ndx+2].fixed_x = True self.lastFootstepPlan.footsteps[ndx+2].fixed_y = True self.lastFootstepPlan.footsteps[ndx+2].fixed_yaw = True self.sendUpdatePlanRequest()
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)