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 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 getDefaultStepParams(self):
default_step_params = lcmdrc.footstep_params_t()
default_step_params.step_speed = self.params.properties.drake_swing_speed
default_step_params.drake_min_hold_time = self.params.properties.drake_min_hold_time
default_step_params.drake_instep_shift = self.params.properties.drake_instep_shift
default_step_params.step_height = self.params.properties.swing_height
default_step_params.constrain_full_foot_pose = True
default_step_params.bdi_step_duration = 2.0
default_step_params.bdi_sway_duration = 0.0
default_step_params.bdi_lift_height = 0.065
default_step_params.bdi_toe_off = 1
default_step_params.bdi_knee_nominal = 0.0
default_step_params.bdi_max_foot_vel = 0.0
default_step_params.bdi_sway_end_dist = 0.02
default_step_params.bdi_step_end_dist = 0.02
default_step_params.mu = 1.0
default_step_params.support_contact_groups = self.params.properties.support_contact_groups
default_step_params.prevent_swing_undershoot = self.params.properties.prevent_swing_undershoot
default_step_params.prevent_swing_overshoot = self.params.properties.prevent_swing_overshoot
return default_step_params
def getDefaultStepParams(self):
default_step_params = lcmdrc.footstep_params_t()
default_step_params.step_speed = self.params.properties.drake_swing_speed
default_step_params.drake_min_hold_time = self.params.properties.drake_min_hold_time
default_step_params.drake_instep_shift = self.params.properties.drake_instep_shift
default_step_params.step_height = self.params.properties.swing_height
default_step_params.constrain_full_foot_pose = True
default_step_params.bdi_step_duration = 2.0
default_step_params.bdi_sway_duration = 0.0
default_step_params.bdi_lift_height = 0.065
default_step_params.bdi_toe_off = 1
default_step_params.bdi_knee_nominal = 0.0
default_step_params.bdi_max_foot_vel = 0.0
default_step_params.bdi_sway_end_dist = 0.02
default_step_params.bdi_step_end_dist = 0.02
default_step_params.mu = 1.0
default_step_params.support_contact_groups = self.params.properties.support_contact_groups
default_step_params.prevent_swing_undershoot = self.params.properties.prevent_swing_undershoot
default_step_params.prevent_swing_overshoot = self.params.properties.prevent_swing_overshoot
return default_step_params
