Esempio n. 1
0
 def state_to_yaml(self, state):
     my_pose, my_vel = state
     self.pose_space.belongs(my_pose)
     self.pose_space.algebra.belongs(my_vel)
     pose = self.pose_space.embed_in(SE3, my_pose)
     vel = self.pose_space.algebra.embed_in(se3, my_vel)
     return to_yaml('TSE3', (pose, vel))
Esempio n. 2
0
 def state_to_yaml(self, state):
     my_pose, my_vel = state
     self.pose_space.belongs(my_pose)
     self.pose_space.algebra.belongs(my_vel)
     pose = self.pose_space.embed_in(SE3, my_pose)
     vel = self.pose_space.algebra.embed_in(se3, my_vel)
     return to_yaml('TSE3', (pose, vel))
Esempio n. 3
0
    def to_yaml(self):
        # pose, velocity
        configuration = self.get_configuration()
        pose = configuration[0]

        joints = []
        for i in range(self.dynamics.num_joints()):
            jc = self.dynamics.joint_state(self._get_state(), i)
            joints.append(to_yaml("TSE3", jc))

        data = {
            'radius': self.radius,
            'id_sensors': self.id_sensors,
            'id_dynamics': self.id_dynamics,
            'pose': SE3.to_yaml(pose),
            'conf': to_yaml('TSE3', configuration),
            'state': self.dynamics.state_to_yaml(self._get_state()),
            'joints': joints,
            'sensors': [s.to_yaml() for s in self.sensors],
            'extra': self.extra
        }
        check_yaml_friendly(data)
        return data
Esempio n. 4
0
    def to_yaml(self):
        # pose, velocity
        configuration = self.get_configuration()
        pose = configuration[0]

        joints = []
        for i in range(self.dynamics.num_joints()):
            jc = self.dynamics.joint_state(self._get_state(), i)
            joints.append(to_yaml("TSE3", jc))

        data = {
            'radius': self.radius,
            'id_sensors': self.id_sensors,
            'id_dynamics': self.id_dynamics,
            'pose': SE3.to_yaml(pose),
            'conf': to_yaml('TSE3', configuration),
            'state': self.dynamics.state_to_yaml(self._get_state()),
            'joints': joints,
            'sensors': [s.to_yaml() for s in self.sensors],
            'extra': self.extra
        }
        check_yaml_friendly(data)
        return data
Esempio n. 5
0
 def to_yaml(self, x):
     # TODO: add testing
     from geometry.yaml import to_yaml
     return to_yaml('%s' % self, x)