def to_bdi_foot_data(self):
foot_data = 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 bdi_step_queue_out(self):
bdi_step_queue_out = [s.copy() for s in self.bdi_step_queue_in]
for step in bdi_step_queue_out:
# Transform to BDI coordinate frame
T1 = ut.mk_transform(step.pos[:3], step.pos[3:])
T2 = ut.mk_transform(self.T_local_to_localbdi.trans,
ut.quat2rpy(self.T_local_to_localbdi.quat))
T = T2.dot(T1)
step.pos[:3] = T[:3, 3]
step.pos[3:] = ut.rotmat2rpy(T[:3, :3])
self.lc.publish(
'BDI_ADJUSTED_FOOTSTEP_PLAN',
encode_footstep_plan(bdi_step_queue_out,
self.last_params).encode())
for step in bdi_step_queue_out:
# Express pos of the center of the foot, as expected by BDI
R = ut.rpy2rotmat(step.pos[3:])
offs = R.dot(ATLAS_FRAME_OFFSET)
# import pdb; pdb.set_trace()
step.pos[:3] += offs
for i in reversed(range(2, len(bdi_step_queue_out))):
bdi_step_queue_out[i].pos[2] -= bdi_step_queue_out[i - 1].pos[2]
return [
s.to_bdi_spec(self.behavior, j + 1)
for j, s in enumerate(bdi_step_queue_out[2:])
]
def to_bdi_foot_data(self):
foot_data = 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 bdi_step_queue_out(self):
bdi_step_queue_out = [s.copy() for s in self.bdi_step_queue_in]
for step in bdi_step_queue_out:
# Transform to BDI coordinate frame
T1 = ut.mk_transform(step.pos[:3], step.pos[3:])
T2 = ut.mk_transform(self.T_local_to_localbdi.trans, ut.quat2rpy(self.T_local_to_localbdi.quat))
T = T2.dot(T1)
step.pos[:3] = T[:3,3]
step.pos[3:] = ut.rotmat2rpy(T[:3,:3])
self.lc.publish('BDI_ADJUSTED_FOOTSTEP_PLAN', encode_footstep_plan(bdi_step_queue_out, self.last_params).encode())
for step in bdi_step_queue_out:
# Express pos of the center of the foot, as expected by BDI
R = ut.rpy2rotmat(step.pos[3:])
offs = R.dot(ATLAS_FRAME_OFFSET)
# import pdb; pdb.set_trace()
step.pos[:3] += offs
for i in reversed(range(2, len(bdi_step_queue_out))):
bdi_step_queue_out[i].pos[2] -= bdi_step_queue_out[i-1].pos[2]
return [s.to_bdi_spec(self.behavior, j+1) for j, s in enumerate(bdi_step_queue_out[2:])]