def to_footstep_t(self): msg = drc.footstep_t() msg.pos = drc.position_3d_t() msg.pos.translation = drc.vector_3d_t() msg.pos.translation.x, msg.pos.translation.y, msg.pos.translation.z = self.pos[:3] msg.pos.rotation = drc.quaternion_t() msg.pos.rotation.w, msg.pos.rotation.x, msg.pos.rotation.y, msg.pos.rotation.z = ut.rpy2quat(self.pos[3:]) msg.id = self.step_id msg.is_right_foot = self.is_right_foot msg.is_in_contact = self.is_in_contact msg.fixed_x, msg.fixed_y, msg.fixed_z, msg.fixed_roll, msg.fixed_pitch, msg.fixed_yaw = self.pos_fixed msg.num_terrain_pts = self.terrain_pts.shape[1] if msg.num_terrain_pts > 0: msg.terrain_path_dist = self.terrain_pts[0, :] msg.terrain_height = self.terrain_pts[1, :] msg.params = drc.footstep_params_t() # TODO: this should probably be filled in msg.params.bdi_step_duration = self.bdi_step_duration msg.params.bdi_sway_duration = self.bdi_sway_duration msg.params.bdi_lift_height = self.bdi_lift_height msg.params.bdi_toe_off = self.bdi_toe_off msg.params.bdi_knee_nominal = self.bdi_knee_nominal msg.params.bdi_max_body_accel = self.bdi_max_body_accel msg.params.bdi_max_foot_vel = self.bdi_max_foot_vel msg.params.bdi_sway_end_dist = self.bdi_sway_end_dist msg.params.bdi_step_end_dist = self.bdi_step_end_dist msg.params.support_contact_groups = self.support_contact_groups return msg
def to_footstep_t(self): msg = drc.footstep_t() msg.pos = bot_core.position_3d_t() msg.pos.translation = bot_core.vector_3d_t() msg.pos.translation.x, msg.pos.translation.y, msg.pos.translation.z = self.pos[: 3] msg.pos.rotation = bot_core.quaternion_t() msg.pos.rotation.w, msg.pos.rotation.x, msg.pos.rotation.y, msg.pos.rotation.z = ut.rpy2quat( self.pos[3:]) msg.id = self.step_id msg.is_right_foot = self.is_right_foot msg.is_in_contact = self.is_in_contact msg.fixed_x, msg.fixed_y, msg.fixed_z, msg.fixed_roll, msg.fixed_pitch, msg.fixed_yaw = self.pos_fixed msg.num_terrain_pts = self.terrain_pts.shape[1] if msg.num_terrain_pts > 0: msg.terrain_path_dist = self.terrain_pts[0, :] msg.terrain_height = self.terrain_pts[1, :] msg.params = drc.footstep_params_t() # TODO: this should probably be filled in msg.params.bdi_step_duration = self.bdi_step_duration msg.params.bdi_sway_duration = self.bdi_sway_duration msg.params.bdi_lift_height = self.bdi_lift_height msg.params.bdi_toe_off = self.bdi_toe_off msg.params.bdi_knee_nominal = self.bdi_knee_nominal msg.params.bdi_max_body_accel = self.bdi_max_body_accel msg.params.bdi_max_foot_vel = self.bdi_max_foot_vel msg.params.bdi_sway_end_dist = self.bdi_sway_end_dist msg.params.bdi_step_end_dist = self.bdi_step_end_dist msg.params.support_contact_groups = self.support_contact_groups return msg
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 generate_plan(self, behavior): plan = drc.footstep_plan_t() plan.utime = 0 plan.params = drc.footstep_plan_params_t() plan.params.behavior = behavior plan.footsteps = [] plan.num_steps = 10 for j in range(plan.num_steps): goal = drc.footstep_t() goal.utime = 0 goal.pos = drc.position_3d_t() goal.pos.translation = drc.vector_3d_t() goal.params = drc.footstep_params_t() if 1 <= j <= 2: # this works in Python! goal.pos.translation.x = 0 goal.pos.translation.y = 0 goal.pos.translation.z = 0 goal.pos.rotation = drc.quaternion_t() goal.pos.rotation.x = 0 goal.pos.rotation.y = 0 goal.pos.rotation.z = 0 goal.pos.rotation.w = 1 else: goal.pos.translation.x = 0.15 * j + (0.5 - random.random()) * 0.15 goal.pos.translation.y = random.random() * 0.15 * j + ( 0.5 - random.random()) * 0.2 goal.pos.translation.z = (0.5 - random.random()) * 0.2 goal.pos.rotation = drc.quaternion_t() goal.pos.rotation.x = random.random() goal.pos.rotation.y = random.random() goal.pos.rotation.z = random.random() goal.pos.rotation.w = random.random() goal.id = j + 1 goal.is_right_foot = j % 2 == 0 goal.is_in_contact = True goal.fixed_x = True goal.fixed_y = True goal.fixed_z = True goal.fixed_roll = True goal.fixed_pitch = True goal.fixed_yaw = True goal.params.step_speed = 1.5 goal.params.step_height = random.random() * 0.25 goal.params.bdi_step_duration = 2.0 goal.params.bdi_sway_duration = 0 goal.params.bdi_lift_height = random.random() * 0.15 goal.params.bdi_toe_off = 0 goal.params.bdi_knee_nominal = 0 goal.num_terrain_pts = 3 goal.terrain_path_dist = [0, 0.1, 0.2] goal.terrain_height = [0, random.random() * 0.15, 0] plan.footsteps.append(goal) return plan
def generate_plan(self, behavior): plan = drc.footstep_plan_t() plan.utime = 0 plan.params = drc.footstep_plan_params_t() plan.params.behavior = behavior plan.footsteps = [] plan.num_steps = 10 for j in range(plan.num_steps): goal = drc.footstep_t() goal.utime = 0 goal.pos = drc.position_3d_t(); goal.pos.translation = drc.vector_3d_t() goal.params = drc.footstep_params_t() if 1 <= j <= 2: # this works in Python! goal.pos.translation.x = 0 goal.pos.translation.y = 0 goal.pos.translation.z = 0 goal.pos.rotation = drc.quaternion_t() goal.pos.rotation.x = 0 goal.pos.rotation.y = 0 goal.pos.rotation.z = 0 goal.pos.rotation.w = 1 else: goal.pos.translation.x = 0.15 * j + (0.5 - random.random()) * 0.15 goal.pos.translation.y = random.random() * 0.15 * j + (0.5 - random.random()) * 0.2 goal.pos.translation.z = (0.5 - random.random()) * 0.2 goal.pos.rotation = drc.quaternion_t() goal.pos.rotation.x = random.random() goal.pos.rotation.y = random.random() goal.pos.rotation.z = random.random() goal.pos.rotation.w = random.random() goal.id = j+1 goal.is_right_foot = j % 2 == 0 goal.is_in_contact = True goal.fixed_x = True goal.fixed_y = True goal.fixed_z = True goal.fixed_roll = True goal.fixed_pitch= True goal.fixed_yaw = True goal.params.step_speed = 1.5 goal.params.step_height = random.random() * 0.25 goal.params.bdi_step_duration = 2.0 goal.params.bdi_sway_duration = 0 goal.params.bdi_lift_height = random.random() * 0.15 goal.params.bdi_toe_off = 0 goal.params.bdi_knee_nominal = 0 goal.num_terrain_pts = 3 goal.terrain_path_dist = [0, 0.1, 0.2] goal.terrain_height = [0, random.random() * 0.15, 0] plan.footsteps.append(goal) return plan
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 = 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 = 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 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 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 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 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 request_handler(channel, data): request = drc.footstep_plan_request_t.decode(data) # ----- Send LCM message based on received info ----- # Create Footstep planner and get solution f = FootstepPlanner(2) # Create Atlas object print("----------------------------------") pm = PackageMap() model = os.path.abspath( os.path.join(os.sep, "home", "rahuly", "Data", "drake-new", "drake-distro", "drake", "examples", "atlas", "urdf", "atlas_minimal_contact.urdf")) pm.PopulateUpstreamToDrake(model) robot = rbtree.RigidBodyTree( model, package_map=pm, floating_base_type=rbtree.FloatingBaseType.kQuaternion) t = request.initial_state.pose.translation r = request.initial_state.pose.rotation q = [t.x, t.y, t.z] + [r.w, r.x, r.y, r.z] + list( request.initial_state.joint_position) q = tuple(q) v = [0] * 6 + list(request.initial_state.joint_velocity) v = tuple(v) kinsol = robot.doKinematics(q, v) r_startpos = robot.transformPoints( kinsol, np.zeros((3, 1)), robot.findFrame("r_foot_sole").get_frame_index(), 0) l_startpos = robot.transformPoints( kinsol, np.zeros((3, 1)), robot.findFrame("l_foot_sole").get_frame_index(), 0) print("r_startpos: " + str(r_startpos)) print("l_startpos: " + str(l_startpos)) print("----------------------------------") f.setStartRL(r_startpos, l_startpos) f.setGoal( [request.goal_pos.translation.x, request.goal_pos.translation.y]) f.setReachable([(0, -0.2), (0.5, 0), (0, 0.2), (-0.3, 0)], 0.3) # f.setNominal(0.5) f.solveProgram() # Package solution into footstep_plan_t plan = drc.footstep_plan_t() # - Footsteps plan.num_steps = 2 * f.numFootsteps for fNum in range(0, f.numFootsteps): # -- Right Footstep rstep = drc.footstep_t() rstep.pos.rotation.w = 1 rspos = np.array([[f.footsteps[2 * fNum][0]], [f.footsteps[2 * fNum][1]], [0]]) # rs = np.array([f.footsteps[2*fNum][0], f.footsteps[2*fNum][1], 0]) rs = robot.transformPoints( kinsol, rspos, robot.FindBody("r_foot").get_body_index(), robot.findFrame("r_foot_sole").get_frame_index()) print("Right: " + str(rs)) rstep.pos.translation.x = rs[0] rstep.pos.translation.y = rs[1] rstep.pos.translation.z = rs[2] # pos.rotation ?? rstep.id = 2 * fNum rstep.is_right_foot = True rstep.params = request.default_step_params plan.footsteps.append(rstep) # -- Left Footstep lstep = drc.footstep_t() lstep.pos.rotation.w = 1 lspos = np.array([[f.footsteps[2 * fNum + 1][0]], [f.footsteps[2 * fNum + 1][1]], [0]]) # ls = np.array([f.footsteps[2*fNum+1][0], f.footsteps[2*fNum+1][1], 0]) ls = robot.transformPoints( kinsol, lspos, robot.FindBody("l_foot").get_body_index(), robot.findFrame("l_foot_sole").get_frame_index()) print("Left: " + str(ls)) lstep.pos.translation.x = ls[0] lstep.pos.translation.y = ls[1] lstep.pos.translation.z = ls[2] # pos.rotation ?? lstep.id = 2 * fNum + 1 lstep.is_right_foot = False lstep.params = request.default_step_params plan.footsteps.append(lstep) # - IRIS Regions plan.num_iris_regions = request.num_iris_regions plan.iris_regions = request.iris_regions plan.iris_region_assignments = [-1] * plan.num_steps # - Params: AFTER WE HAVE REQUESTS WORKING plan.params = request.params # Send footstep_plan_t lc.publish("FOOTSTEP_PLAN", plan.encode())