def __init__(self, mode=Mode.translating, safe=True):
self.mode = mode
self.safe = safe # Don't send atlas behavior commands (to ensure that the robot never starts walking accidentally when running tests)
self.lc = lcm.LCM()
if self.mode == Mode.plotting:
self.gl = gl
else:
self.gl = None
self.bdi_step_queue_in = []
self.delivered_index = None
self.use_spec = True
self.drift_from_plan = np.zeros((3,1))
self.behavior = Behavior.BDI_STEPPING
self.T_local_to_localbdi = bot_core.rigid_transform_t()
self.T_local_to_localbdi.trans = np.zeros(3)
self.T_local_to_localbdi.quat = ut.rpy2quat([0,0,0])
self.last_params = None
self.executing = False
self.last_footstep_plan_time = -np.inf
def __init__(self, mode=Mode.translating, safe=True):
self.mode = mode
self.safe = safe # Don't send atlas behavior commands (to ensure that the robot never starts walking accidentally when running tests)
self.lc = lcm.LCM()
if self.mode == Mode.plotting:
self.gl = gl
else:
self.gl = None
self.bdi_step_queue_in = []
self.delivered_index = None
self.use_spec = True
self.drift_from_plan = np.zeros((3, 1))
self.behavior = Behavior.BDI_STEPPING
self.T_local_to_localbdi = bot_core.rigid_transform_t()
self.T_local_to_localbdi.trans = np.zeros(3)
self.T_local_to_localbdi.quat = ut.rpy2quat([0, 0, 0])
self.last_params = None
self.executing = False
self.last_footstep_plan_time = -np.inf
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
