def servo_stats_summary(data_central, id_agent, id_robot, id_episode): # @UnusedVariable
from geometry import (SE2, SE2_from_SE3, translation_from_SE2,
angle_from_SE2, SE3)
log_index = data_central.get_log_index()
errors = []
timestamps = []
poses = []
dist_xy = []
dist_th = []
for observations in \
log_index.read_robot_episode(id_robot, id_episode,
read_extra=True):
extra = observations['extra'].item()
servoing = extra.get('servoing', None)
if servoing is None:
logger.error('Warning, no "servoing" in episode %r.' %
id_episode)
break
obs0 = np.array(servoing['obs0'])
pose0 = SE2_from_SE3(SE3.from_yaml(servoing['pose0']))
pose1 = SE2_from_SE3(SE3.from_yaml(servoing['pose1']))
poseK = SE2_from_SE3(SE3.from_yaml(servoing['poseK']))
pose1 = SE2.multiply(SE2.inverse(pose0), pose1)
poseK = SE2.multiply(SE2.inverse(pose0), poseK)
poses.append(poseK)
dist_xy.append(np.linalg.norm(translation_from_SE2(poseK)))
dist_th.append(np.abs(angle_from_SE2(poseK)))
# obs1 = np.array(servoing['obs1'])
obsK = np.array(servoing['obsK'])
err_L2 = np.linalg.norm(obs0 - obsK)
errors.append(err_L2)
timestamps.append(observations['timestamp'])
# last['time_from_episode_start'] = observations['time_from_episode_start']
initial_distance = np.linalg.norm(translation_from_SE2(pose1))
summary = {}
summary['pose0'] = pose0
summary['pose1'] = pose1
summary['poses'] = poses
summary['errors'] = errors
summary['timestamps'] = timestamps
summary['initial_translation'] = translation_from_SE2(pose1)
summary['initial_distance'] = initial_distance
summary['initial_rotation'] = angle_from_SE2(pose1)
summary['dist_xy'] = dist_xy
summary['dist_th'] = dist_th
return summary
def update(self):
pose = self.input.pose
if self.state.pose0 is None:
self.state.pose0 = pose
rel_pose = SE2.multiply(SE2.inverse(self.state.pose0), pose)
self.output.rel_pose = rel_pose
def update(self):
pose = self.input.pose
if self.state.pose0 is None:
self.state.pose0 = pose
rel_pose = SE2.multiply(SE2.inverse(self.state.pose0), pose)
self.output.rel_pose = rel_pose
def velocity_from_poses(t1: float, q1: SE2v, t2: float, q2: SE2v) -> se2v:
delta = t2 - t1
if not delta > 0:
raise ValueError('invalid sequence')
x = SE2.multiply(SE2.inverse(q1), q2)
xt = SE2.algebra_from_group(x)
v = xt / delta
return v
def velocity_from_poses(t1: float, q1: SE2value, t2: float,
q2: SE2value) -> SE2value:
delta = t2 - t1
if not delta > 0:
msg = f"invalid delta {delta}"
raise ValueError(msg)
x = SE2.multiply(SE2.inverse(q1), q2)
xt = SE2.algebra_from_group(x)
v = xt / delta
return v
def simulate(self, dt, vehicle):
vehicle_pose = SE2_from_SE3(vehicle.get_pose())
target_pose = self.get_target_pose()
relative_pose = SE2.multiply(SE2.inverse(target_pose), vehicle_pose)
t, theta = translation_angle_from_SE2(relative_pose) #@UnusedVariable
# position on field of view
phi = np.arctan2(t[1], t[0])
diff = normalize_pi_scalar(self.target_stabilize_phi - phi)
# torque command
command = -np.sign(diff)
commands = [command]
self.target_state = self.target_dynamics.integrate(self.target_state,
commands, dt)
self.update_primitives()
return [self.target]
def draw(self, cr, joints=None, timestamp=None):
for js in self.joint_skins:
jointnum = js.get('joint', 0)
if not(0 <= jointnum < len(joints)):
msg = ('Invalid joint number #%d. Dynamics has '
'%d joints.' % (jointnum, len(joints)))
raise ValueError(msg)
skin = js.get('skin')
pose = js.get('pose', [0, 0, 0]) # TODO: honor this
skin_impl = get_conftools_skins().instance(skin)
robot_pose = SE2_from_SE3(joints[0][0])
joint_pose = SE2_from_SE3(joints[jointnum][0])
relative_pose = SE2.multiply(SE2.inverse(robot_pose), joint_pose)
# print('plotting skin %r at rel pose %r' %
# (skin, SE2.friendly(relative_pose)))
with cairo_rototranslate(cr, relative_pose):
skin_impl.draw(cr, timestamp=timestamp)
def draw(self, cr, joints=None, timestamp=None):
for js in self.joint_skins:
jointnum = js.get('joint', 0)
if not (0 <= jointnum < len(joints)):
msg = ('Invalid joint number #%d. Dynamics has '
'%d joints.' % (jointnum, len(joints)))
raise ValueError(msg)
skin = js.get('skin')
pose = js.get('pose', [0, 0, 0]) # TODO: honor this
skin_impl = get_conftools_skins().instance(skin)
robot_pose = SE2_from_SE3(joints[0][0])
joint_pose = SE2_from_SE3(joints[jointnum][0])
relative_pose = SE2.multiply(SE2.inverse(robot_pose), joint_pose)
# print('plotting skin %r at rel pose %r' %
# (skin, SE2.friendly(relative_pose)))
with cairo_rototranslate(cr, relative_pose):
skin_impl.draw(cr, timestamp=timestamp)
def align(poses):
start = poses[0]
poses2 = [SE2.multiply(SE2.inverse(start), p) for p in poses]
return poses2
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2), SE2.multiply(q2, q1))
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2),
SE2.multiply(q2, q1))