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)
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.')
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)
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
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
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)
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)
def angular_from_se2(x: se2v) -> float:
_, angular = geo.linear_angular_from_se2(x)
return angular
def linear_from_se2(x: se2v) -> float:
linear, _ = geo.linear_angular_from_se2(x)
return linear[0]
def lateral(x: se2value) -> float:
l, omega_ = linear_angular_from_se2(x)
return l[1]
def omega(x: se2value) -> float:
l, omega_ = linear_angular_from_se2(x)
return np.rad2deg(omega_)
def speed(x: se2value) -> float:
l, omega_ = linear_angular_from_se2(x)
return l[0]
def angular_from_se2(x: geo.se2value) -> float:
_, angular = geo.linear_angular_from_se2(x)
return angular