def to_bdi_foot_data(self):
foot_data = drc.atlas_behavior_foot_data_t()
# foot_data.position = self.to_atlas_frame(self.pos)[:3]
foot_data.position = self.pos[:3]
foot_data.yaw = self.pos[5]
foot_data.normal = ut.rpy2rotmat(self.pos[3:]).dot(np.array([0,0,1]))
return foot_data
def blank_walk_spec():
msg = drc.atlas_behavior_walk_spec_t()
msg.foot = drc.atlas_behavior_foot_data_t()
msg.action = drc.atlas_behavior_walk_action_t()
return msg
