1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
|
"""This file implements an accurate motor model."""
import numpy as np
VOLTAGE_CLIPPING = 50
# TODO(b/73728631): Clamp the pwm signal instead of the OBSERVED_TORQUE_LIMIT.
OBSERVED_TORQUE_LIMIT = 5.7
MOTOR_VOLTAGE = 16.0
MOTOR_RESISTANCE = 0.186
MOTOR_TORQUE_CONSTANT = 0.0954
MOTOR_VISCOUS_DAMPING = 0
MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (
MOTOR_VISCOUS_DAMPING + MOTOR_TORQUE_CONSTANT)
NUM_MOTORS = 8
class MotorModel(object):
"""The accurate motor model, which is based on the physics of DC motors.
The motor model support two types of control: position control and torque
control. In position control mode, a desired motor angle is specified, and a
torque is computed based on the internal motor model. When the torque control
is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
torque.
The internal motor model takes the following factors into consideration:
pd gains, viscous friction, back-EMF voltage and current-torque profile.
"""
def __init__(self, torque_control_enabled=False, kp=1.2, kd=0):
self._torque_control_enabled = torque_control_enabled
self._kp = kp
self._kd = kd
self._resistance = MOTOR_RESISTANCE
self._voltage = MOTOR_VOLTAGE
self._torque_constant = MOTOR_TORQUE_CONSTANT
self._viscous_damping = MOTOR_VISCOUS_DAMPING
self._current_table = [0, 10, 20, 30, 40, 50, 60]
self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]
self._strength_ratios = [1.0] * NUM_MOTORS
def set_strength_ratios(self, ratios):
"""Set the strength of each motors relative to the default value.
Args:
ratios: The relative strength of motor output. A numpy array ranging from
0.0 to 1.0.
"""
self._strength_ratios = np.array(ratios)
def set_motor_gains(self, kp, kd):
"""Set the gains of all motors.
These gains are PD gains for motor positional control. kp is the
proportional gain and kd is the derivative gain.
Args:
kp: proportional gain of the motors.
kd: derivative gain of the motors.
"""
self._kp = kp
self._kd = kd
def set_voltage(self, voltage):
self._voltage = voltage
def get_voltage(self):
return self._voltage
def set_viscous_damping(self, viscous_damping):
self._viscous_damping = viscous_damping
def get_viscous_dampling(self):
return self._viscous_damping
def convert_to_torque(self,
motor_commands,
motor_angle,
motor_velocity,
true_motor_velocity,
kp=None,
kd=None):
"""Convert the commands (position control or torque control) to torque.
Args:
motor_commands: The desired motor angle if the motor is in position
control mode. The pwm signal if the motor is in torque control mode.
motor_angle: The motor angle observed at the current time step. It is
actually the true motor angle observed a few milliseconds ago (pd
latency).
motor_velocity: The motor velocity observed at the current time step, it
is actually the true motor velocity a few milliseconds ago (pd latency).
true_motor_velocity: The true motor velocity. The true velocity is used
to compute back EMF voltage and viscous damping.
kp: Proportional gains for the motors' PD controllers. If not provided, it
uses the default kp of the minitaur for all the motors.
kd: Derivative gains for the motors' PD controllers. If not provided, it
uses the default kp of the minitaur for all the motors.
Returns:
actual_torque: The torque that needs to be applied to the motor.
observed_torque: The torque observed by the sensor.
"""
if self._torque_control_enabled:
pwm = motor_commands
else:
if kp is None:
kp = np.full(NUM_MOTORS, self._kp)
if kd is None:
kd = np.full(NUM_MOTORS, self._kd)
pwm = -1 * kp * (motor_angle - motor_commands) - kd * motor_velocity
pwm = np.clip(pwm, -1.0, 1.0)
return self._convert_to_torque_from_pwm(pwm, true_motor_velocity)
def _convert_to_torque_from_pwm(self, pwm, true_motor_velocity):
"""Convert the pwm signal to torque.
Args:
pwm: The pulse width modulation.
true_motor_velocity: The true motor velocity at the current moment. It is
used to compute the back EMF voltage and the viscous damping.
Returns:
actual_torque: The torque that needs to be applied to the motor.
observed_torque: The torque observed by the sensor.
"""
observed_torque = np.clip(
self._torque_constant *
(np.asarray(pwm) * self._voltage / self._resistance),
-OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT)
# Net voltage is clipped at 50V by diodes on the motor controller.
voltage_net = np.clip(
np.asarray(pwm) * self._voltage -
(self._torque_constant + self._viscous_damping) *
np.asarray(true_motor_velocity), -VOLTAGE_CLIPPING, VOLTAGE_CLIPPING)
current = voltage_net / self._resistance
current_sign = np.sign(current)
current_magnitude = np.absolute(current)
# Saturate torque based on empirical current relation.
actual_torque = np.interp(current_magnitude, self._current_table,
self._torque_table)
actual_torque = np.multiply(current_sign, actual_torque)
actual_torque = np.multiply(self._strength_ratios, actual_torque)
return actual_torque, observed_torque
|
