示例#1
0
    def integrate(self, dt: float, commands: PWMCommands) -> 'DynamicModel':
        # previous velocities (v0)
        linear_angular_prev = geo.linear_angular_from_se2(self.v0)
        linear_prev = linear_angular_prev[0]
        longit_prev = linear_prev[0]
        lateral_prev = linear_prev[1]
        angular_prev = linear_angular_prev[1]

        # predict the acceleration of the vehicle
        x_dot_dot = self.model(commands,
                               self.parameters,
                               u=longit_prev,
                               w=angular_prev)

        # convert the acceleration to velocity by forward euler
        longitudinal = longit_prev + dt * x_dot_dot[0]
        angular = angular_prev + dt * x_dot_dot[1]
        lateral = 0.0

        linear = [longitudinal, lateral]

        # represent this as se(2)
        commands_se2 = geo.se2_from_linear_angular(linear, angular)

        # call the "integrate" function of GenericKinematicsSE2
        s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)

        # new state
        c1 = s1.q0, s1.v0
        t1 = s1.t0

        return DynamicModel(self.parameters, c1, t1)
示例#2
0
def plot_poses_vels_theta_omega(pylab, poses, vels):
    thetas = np.array([angle_from_SE2(p) for p in poses])
    omegas = np.array([linear_angular_from_se2(vel)[1] for vel in vels])
    positive = omegas > 0
    negative = np.logical_not(positive)
    pylab.plot(np.rad2deg(thetas)[positive], omegas[positive], 'r.')
    pylab.plot(np.rad2deg(thetas)[negative], omegas[negative], 'b.')
示例#3
0
def get_vxy_world(pose, vel):
    (x, y), theta = translation_angle_from_SE2(pose)
    _, omega = linear_angular_from_se2(vel)
    vel2 = np.dot(pose, vel)
    vx = vel2[0, 2]
    vy = vel2[1, 2]
    return x, y, theta, vx, vy, omega
    def integrate(self, dt: float, commands: PWMCommands) -> "DynamicModel":
        # previous velocities (v0)
        linear_angular_prev = geo.linear_angular_from_se2(self.v0)
        linear_prev = linear_angular_prev[0]
        longit_prev = linear_prev[0]
        lateral_prev = linear_prev[1]
        angular_prev = linear_angular_prev[1]

        # predict the acceleration of the vehicle
        x_dot_dot = self.model(commands,
                               self.parameters,
                               u=longit_prev,
                               w=angular_prev)

        # convert the acceleration to velocity by forward euler
        longitudinal = longit_prev + dt * x_dot_dot[0]
        angular = angular_prev + dt * x_dot_dot[1]
        lateral = 0.0

        linear = [longitudinal, lateral]

        # represent this as se(2)
        commands_se2 = geo.se2_from_linear_angular(linear, angular)

        # call the "integrate" function of GenericKinematicsSE2
        s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)

        # new state
        c1 = s1.q0, s1.v0
        t1 = s1.t0

        # now we compute the axis rotation using the inverse way...
        # forward = both wheels spin positive
        # angular_velocity = wR*R_r/d - Wl*R_l/d   # if R rotates more, we increase theta
        # linear_velocity = (wR*R_r + Wl*R_l)/2

        # that is
        # [ang, lin ] = [ Rr/d -Rl/d; Rr/2 Rl/2] * [wR wL]
        d = self.parameters.wheel_distance
        Rr = self.parameters.wheel_radius_right
        Rl = self.parameters.wheel_radius_left
        M = np.array([[Rr / d, -Rl / d], [Rr / 2, Rl / 2]])
        anglin = np.array((angular, longitudinal))
        MInv = np.linalg.inv(M)
        wRL = MInv @ anglin
        wR = float(wRL[0, 0])
        wL = float(wRL[1, 0])

        axis_left_rad = self.axis_left_rad + wL * dt
        axis_right_rad = self.axis_right_rad + wR * dt

        return DynamicModel(self.parameters,
                            c1,
                            t1,
                            axis_left_rad=axis_left_rad,
                            axis_right_rad=axis_right_rad)
 def initialize(cls, c0, t0=0, seed=None):
     """
         This class initializes the dynamics at a given configuration
     """
     # pose, velocity in SE(2), se(2)
     q, v = c0
     # get position p from pose
     p, theta = geo.translation_angle_from_SE2(q)
     # get linear velocity from se(2)
     v2d, _ = geo.linear_angular_from_se2(v)
     # create the integrator2d initial state
     return Integrator2D(p, v2d, t0)
示例#6
0
    def update(self):
        q2 = self.get_input(0)
        t2 = self.get_input_timestamp(0)

        if self.state.prev is not None:
            t1, q1 = self.state.prev
            vel = velocity_from_poses(t1, q1, t2, q2, S=SE2)

            # Convertion from se2 to R3
            v, omega = linear_angular_from_se2(vel)
            out = np.array([v[0], v[1], omega])

            self.set_output(0, out, timestamp=t2)

        self.state.prev = t2, q2
示例#7
0
    def update(self):
        q2 = self.get_input(0)
        t2 = self.get_input_timestamp(0)
        
        if self.state.prev is not None:
            t1, q1 = self.state.prev
            vel = velocity_from_poses(t1, q1, t2, q2, S=SE2)

            # Convertion from se2 to R3
            v, omega = linear_angular_from_se2(vel)
            out = np.array([v[0], v[1], omega])

            self.set_output(0, out, timestamp=t2)
            
        self.state.prev = t2, q2
示例#8
0
    def _render(self):
        # set the pose of this robot as the "protagonist"
        if self.render_timestamps:
            q, v = self.unwrapped.state.TSE2_from_state()
            dt = self.current_time - self.render_timestamps[-1]
            linear, angular = geometry.linear_angular_from_se2(v)
            angular_deg = np.rad2deg(angular)

            speed = linear[0]

            dt_max = get_min_render_dt(speed, angular_deg, self.camera_dt)

            do_it = dt >= dt_max
        else:
            do_it = True

        if do_it:
            obs = self.unwrapped.render_obs()
            self.render_observations.append(obs)
            self.render_timestamps.append(self.current_time)
示例#9
0
def get_distance_two(q0, q1):
    g = geo.SE2.multiply(geo.SE2.inverse(q0), q1)
    v = geo.SE2.algebra_from_group(g)
    linear, angular = geo.linear_angular_from_se2(v)
    return np.linalg.norm(linear)
示例#10
0
def angular_from_se2(x: se2v) -> float:
    _, angular = geo.linear_angular_from_se2(x)
    return angular
示例#11
0
def linear_from_se2(x: se2v) -> float:
    linear, _ = geo.linear_angular_from_se2(x)
    return linear[0]
示例#12
0
 def lateral(x: se2value) -> float:
     l, omega_ = linear_angular_from_se2(x)
     return l[1]
示例#13
0
 def omega(x: se2value) -> float:
     l, omega_ = linear_angular_from_se2(x)
     return np.rad2deg(omega_)
示例#14
0
 def speed(x: se2value) -> float:
     l, omega_ = linear_angular_from_se2(x)
     return l[0]
示例#15
0
def angular_from_se2(x: geo.se2value) -> float:
    _, angular = geo.linear_angular_from_se2(x)
    return angular