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