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 send_expected_double_support(self): """ Publish the next expected double support configuration as a two-element footstep plan to support continuous replanning mode. """ self.lc.publish( 'NEXT_EXPECTED_DOUBLE_SUPPORT', encode_footstep_plan( self. bdi_step_queue_in[self.delivered_index:self.delivered_index + 2], self.last_params).encode())
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 send_expected_double_support(self): """ Publish the next expected double support configuration as a two-element footstep plan to support continuous replanning mode. """ self.lc.publish('NEXT_EXPECTED_DOUBLE_SUPPORT', encode_footstep_plan(self.bdi_step_queue_in[self.delivered_index:self.delivered_index+2], self.last_params).encode())