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
Exemple #3
0
    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
Exemple #4
0
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
Exemple #5
0
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]
Exemple #7
0
    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)
Exemple #8
0
    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)
Exemple #9
0
def align(poses):
    start = poses[0]
    poses2 = [SE2.multiply(SE2.inverse(start), p) for p in poses]
    return poses2
Exemple #10
0
 def commuting(a, b):
     q1 = motions[a]
     q2 = motions[b]
     return SE2.distance(SE2.multiply(q1, q2), SE2.multiply(q2, q1))
Exemple #11
0
 def commuting(a, b):
     q1 = motions[a]
     q2 = motions[b]
     return SE2.distance(SE2.multiply(q1, q2),
                         SE2.multiply(q2, q1))