Exemple #1
0
 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
Exemple #2
0
    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:])
        ]
Exemple #3
0
 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
Exemple #4
0
    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:])]