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 encode_footstep_plan(footsteps, params):
plan = drc.footstep_plan_t()
plan.num_steps = len(footsteps)
plan.footsteps = [step.to_footstep_t() for step in footsteps]
if params is not None:
plan.params = params
else:
plan.params = drc.footstep_plan_params_t()
plan.iris_region_assignments = [-1 for f in footsteps]
return plan
def encode_footstep_plan(footsteps, params):
plan = drc.footstep_plan_t()
plan.num_steps = len(footsteps)
plan.footsteps = [step.to_footstep_t() for step in footsteps]
if params is not None:
plan.params = params
else:
plan.params = drc.footstep_plan_params_t()
plan.iris_region_assignments = [-1 for f in footsteps]
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 applyParams(self, msg):
msg.params = lcmdrc.footstep_plan_params_t()
msg.params.max_num_steps = self.params.properties.max_num_steps
msg.params.min_num_steps = self.params.properties.min_num_steps
msg.params.min_step_width = self.params.properties.min_step_width
msg.params.nom_step_width = self.params.properties.nominal_step_width
msg.params.max_step_width = self.params.properties.max_step_width
msg.params.nom_forward_step = self.params.properties.nominal_forward_step
msg.params.max_forward_step = self.params.properties.max_forward_step
msg.params.nom_upward_step = self.params.properties.max_upward_step
msg.params.nom_downward_step = self.params.properties.max_downward_step
msg.params.planning_mode = self.params.properties.planner_mode
msg.params.behavior = self.behavior_lcm_map[self.params.properties.behavior]
# msg.params.use_map_heights = self.params.properties.heights_source == 0
# msg.params.use_map_normals = self.params.properties.normals_source == 0
msg.params.map_mode = self.map_mode_map[self.params.properties.map_mode]
# msg.params.map_command = self.map_command_lcm_map[self.params.properties.map_command]
msg.params.leading_foot = self.leading_foot_map[self.params.properties.leading_foot]
msg.default_step_params = self.getDefaultStepParams()
return msg
def applyParams(self, msg):
msg.params = lcmdrc.footstep_plan_params_t()
msg.params.max_num_steps = self.params.properties.max_num_steps
msg.params.min_num_steps = self.params.properties.min_num_steps
msg.params.min_step_width = self.params.properties.min_step_width
msg.params.nom_step_width = self.params.properties.nominal_step_width
msg.params.max_step_width = self.params.properties.max_step_width
msg.params.nom_forward_step = self.params.properties.nominal_forward_step
msg.params.max_forward_step = self.params.properties.max_forward_step
msg.params.nom_upward_step = self.params.properties.max_upward_step
msg.params.nom_downward_step = self.params.properties.max_downward_step
msg.params.planning_mode = self.params.properties.planner_mode
msg.params.behavior = self.behavior_lcm_map[self.params.properties.behavior]
# msg.params.use_map_heights = self.params.properties.heights_source == 0
# msg.params.use_map_normals = self.params.properties.normals_source == 0
msg.params.map_mode = self.map_mode_map[self.params.properties.map_mode]
# msg.params.map_command = self.map_command_lcm_map[self.params.properties.map_command]
msg.params.leading_foot = self.leading_foot_map[self.params.properties.leading_foot]
msg.default_step_params = self.getDefaultStepParams()
return msg
