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))
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))
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
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
def to_yaml(self, x):
# TODO: add testing
from geometry.yaml import to_yaml
return to_yaml('%s' % self, x)