I'm using OpenRAVE to simulate a quadruped, in order to get an idea of torque requirements.
To get started I made a single DOF, single link pendulum to test controllers etc out on. I've whipped up an inverse dynamics based PD controller using ComputeInverseDynamics(), which I set the outputs using SetDOFTorques(). I then set a desired position, with the desired velocity being zero. This all appears to work well and I can start the simulation, with the pendulum driving up to the desired position and settling. My concern is the value of the output torques. My pendulum is modeled as a simple box of length 1, mass manually set to 1, with a COM of 0.5. When I run my simulation, I output the gravity component from ComputeInverseDynamics(). This gives 4.9NM, which matches up with hand calculated torques I expect from the pendulum (eg the static case) when it is driven to the desired position (from down to horizontal). But the output torques to SetDOFTorques() are much higher and vary depending what I set the simulation timestep to. If I maintain a controller update rate of 0.001 seconds, then for a simulation update of 0.0001 seconds, my output torque is approximately 87NM. If I alter the simulation timestep to 0.0005 seconds, keeping the controller rate the same the output torques drop down to about 18NM.
As an experiment I removed the inverse dynamics controller and replaced it with a plain PD controller, but I still see large output torques.
Can anyone shed some light on this? It's very possible I'm missing something here!
Thanks very much
Edits: I'm adding the main section of my code. There is no trajectory generation, really. I'm just trying to get to a fixed static position. In the code, if I keep dt fixed, and alter env.StartSimulation(timestep=0.0001), I get the issues popping up.
with env:
robot = env.GetRobots()[0]
robot.GetLinks()[0].SetStatic(True)
env.StopSimulation()
env.StartSimulation(timestep=0.0001)
dt = 0.001
w = 100
eta = 5
Kp = [w*w]
Kv = [2*eta*w]
# Desired pos, vel and acc
cmd_p = [3.14/2]
cmd_v = [0]
cmd_a = [0]
while True:
with env:
torqueconfiguration, torquecoriolis, torquegravity = robot.ComputeInverseDynamics([1],None,returncomponents=True)
err_p = cmd_p - robot.GetDOFValues()
err_v = cmd_v - robot.GetDOFVelocities()
# ID Controller
M = compute_inertia_matrix(robot, robot.GetDOFValues())
a_cmd = (Kp*err_p + Kv*err_v + cmd_a)
taus = torquegravity + torquecoriolis + M.dot(a_cmd.transpose()).transpose()
# Just PD(ish) controller
#taus = Kp*err_p - Kv*robot.GetDOFVelocities()
with robot:
robot.SetDOFTorques(taus,False) # True = use limits
print (taus, torquegravity+torquecoriolis, a_cmd, M.dot(a_cmd.transpose()).transpose())
time.sleep(dt)
# https://scaron.info/teaching/equations-of-motion.html
def compute_inertia_matrix(robot, q, external_torque=None):
n = len(q)
M = np.zeros((n, n))
with robot:
robot.SetDOFValues(q)
for (i, e_i) in enumerate(np.eye(n)):
m, c, g = robot.ComputeInverseDynamics(e_i, external_torque, returncomponents=True)
M[:, i] = m
return M
<?xml version="1.0" encoding="utf-8"?>
<Robot name="Pendulum">
<RotationAxis>0 1 0 90</RotationAxis> <!-- makes the pendulum vertical -->
<KinBody>
<!-- <Mass type="mimicgeom"><density>100000</density></Mass> -->
<Body name="Base" type="dynamic">
<Translation>0.0 0.0 0.0</Translation>
<Geom type="cylinder">
<rotationaxis>1 0 0 90</rotationaxis>
<radius>0.3</radius>
<height>0.02</height>
<ambientColor>1. 0. 0.</ambientColor>
<diffuseColor>1. 0. 0.</diffuseColor>
</Geom>
<mass type="custom">
<!-- specify the total mass-->
<total>5.0</total>
<!-- specify the 3x3 inertia matrix-->
<!--<inertia>2 0 0 0 3 0 0 0 5</inertia> -->
<!-- specify the center of mass (if using ODE physics engine, should be 0)-->
<com>0.1 0.0 0.0</com>
</mass>
</Body>
<Body name="Arm0" type="dynamic">
<offsetfrom>Base</offsetfrom>
<!-- translation and rotation will be relative to Base -->
<Translation>0 0 0</Translation>
<Geom type="box">
<Translation>1 0 0</Translation>
<Extents>1 0.1 0.1</Extents>
<ambientColor>1. 0. 0.</ambientColor>
<diffuseColor>1. 0. 0.</diffuseColor>
</Geom>
<mass type="custom">
<!-- specify the total mass-->
<total>1.0</total>
<!-- specify the 3x3 inertia matrix-->
<!--<inertia>2 0 0 0 3 0 0 0 5</inertia> -->
<!-- specify the center of mass (if using ODE physics engine, should be 0)-->
<com>0.5 0.0 0.0</com>
</mass>
</Body>
<Joint circular="true" name="Joint0" type="hinge">
<Body>Base</Body>
<Body>Arm0</Body>
<offsetfrom>Arm0</offsetfrom>
<weight>0</weight>
<axis>0 0 1</axis>
<maxvel>100</maxvel>
<resolution>1</resolution>
</Joint>
</KinBody>
</Robot>
Here is some data for dt = 0.001 and env.StartSimulation(timestep=0.0001)
In this data,
- taus is the torque command to the simulation,
- torquegravity+torquecoriolis is returned from the inverse dynamics
- a_cmd is the controller command and
- M*a_cmd is the command after being multiplied by the mass matrix
The gravity and coriolis parts appear to be correct for steady state, where it should be about 4.9NM
taus, torquegravity+torquecoriolis, a_cmd, M*a_cmd
3464.88331508, 0.48809828, 5329.83879509, 3464.39521681
330.67177959, 1.47549936, 506.45581573, 329.19628023
-785.91806527, 2.45531014, -1212.88211601, -788.37337541
-1065.4689484, 3.23603844, -1644.16151823, -1068.70498685
-1027.47479809, 3.80261774, -1586.58063974, -1031.27741583
-877.83110127, 4.18635604, -1356.94993433, -882.01745731
-707.25108627, 4.4371714, -1094.9050118, -711.68825767
-554.34483533, 4.6006198, -859.91608481, -558.94545512
-432.22314217, 4.70818921, -672.20204828, -436.93133138
-327.797496, 4.7768792, -511.65288492, -332.5743752
-240.77203429, 4.82021019, -377.83422228, -245.59224448
-172.18942128, 4.84807059, -272.3653721, -177.03749186
-117.58895761, 4.86591166, -188.39210657, -122.45486927
-74.51920719, 4.87743369, -122.14867828, -79.39664088
-39.91183436, 4.88473444, -68.91779816, -44.7965688
-12.82321495, 4.88971433, -27.25066043, -17.71292928
8.45349476, 4.89281357, 5.47797105, 3.56068118
25.35468725, 4.89489884, 31.47659755, 20.45978841
38.84080509, 4.896309, 52.22230167, 33.94449609
48.72668147, 4.89724689, 67.42989936, 43.82943458
56.78552877, 4.89790152, 79.82711885, 51.88762725
65.515892, 4.89836756, 93.25772991, 60.61752444
68.81359264, 4.89867903, 98.33063633, 63.91491362
73.86961896, 4.89891052, 106.10878221, 68.97070844
76.67416578, 4.89907489, 110.42321674, 71.77509088
79.62549808, 4.89919702, 114.96354008, 74.72630105
85.17343708, 4.89928669, 123.49869291, 80.27415039
85.13686188, 4.89934963, 123.44232654, 80.23751225
85.75675034, 4.89939931, 124.39592466, 80.85735103
86.55192592, 4.89943807, 125.61921208, 81.65248785
86.39672231, 4.89946802, 125.38039121, 81.49725429
87.4299925, 4.89949202, 126.97000073, 82.53050048
87.42776523, 4.8995098, 126.96654682, 82.52825543
87.15472709, 4.8995251, 126.54646461, 82.255202
86.97240783, 4.89953825, 126.26595319, 82.07286958
86.98023044, 4.89954905, 126.27797137, 82.08068139
86.75364661, 4.89955809, 125.92936696, 81.85408852
86.9853716, 4.89956526, 126.28585591, 82.08580634
88.01679721, 4.89957062, 127.8726563, 83.1172266
89.2610231, 4.89957348, 129.78684557, 84.36144962
88.47969399, 4.89957495, 128.58479851, 83.58011903
88.77623594, 4.89957711, 129.04101359, 83.87665884
90.87280518, 4.89957739, 132.2665043, 85.9732278
88.9513552, 4.89957707, 129.3104279, 84.05177813
89.14100099, 4.89957773, 129.60218964, 84.24142327
And here is some data for dt = 0.001 and env.StartSimulation(timestep=0.0005)
taus, torquegravity+torquecoriolis, a_cmd, M*a_cmd
-313.62240349, 0.98927261, -484.01796324, -314.61167611
-242.03525463, 2.00886997, -375.45249938, -244.0441246
-199.82226305, 2.79259699, -311.71516928, -202.61486003
-190.02605484, 3.39367572, -297.56881625, -193.41973056
-162.08293067, 3.8525617, -255.28537288, -165.93549237
-125.84847045, 4.17559368, -200.03702174, -130.02406413
-103.89936813, 4.40068949, -166.61547326, -108.30005762
-82.32305905, 4.5566127, -133.66103347, -86.87967175
-64.56801352, 4.66415211, -106.51102404, -69.23216563
-49.68124446, 4.73812107, -83.72210081, -54.41936553
-37.91265825, 4.78890663, -65.6947152, -42.70156488
-27.99189838, 4.82374208, -50.48560071, -32.81564046
-19.81225948, 4.84762415, -37.9382825, -24.65988362
-12.55978349, 4.8636252, -26.80524414, -17.42340869
-6.89165107, 4.87470983, -18.10209369, -11.7663609
-3.13313345, 4.88256746, -12.33184754, -8.0157009
0.69831646, 4.88796162, -6.44560793, -4.18964516
3.86277859, 4.89166745, -1.58290594, -1.02888886
6.12163439, 4.8941598, 1.88842245, 1.22747459
8.58189707, 4.89593332, 5.67071346, 3.68596375
9.1580546, 4.89712981, 6.55526891, 4.26092479
11.81854706, 4.89798468, 10.64701905, 6.92056238
12.40540565, 4.89856409, 11.54898701, 7.50684156
14.04109075, 4.89897979, 14.06478609, 9.14211096
14.39924399, 4.89926951, 14.61534535, 9.49997448
14.98060951, 4.89947252, 15.50944153, 10.08113699
16.08890875, 4.89961544, 17.2142974, 11.18929331
16.01955973, 4.89971637, 17.10745133, 11.11984337
17.06493791, 4.89978831, 18.71561478, 12.16514961
17.35364328, 4.89983976, 19.15969772, 12.45380352
17.62239334, 4.89987688, 19.57310225, 12.72251646
17.84455913, 4.89990387, 19.91485424, 12.94465525
17.43825648, 4.89992362, 19.28974286, 12.53833286
17.58436934, 4.89993826, 19.51450935, 12.68443108
17.70571012, 4.8999492, 19.70117065, 12.80576093
18.40852272, 4.89995746, 20.78240808, 13.50856525
18.49492461, 4.89996372, 20.91532445, 13.59496089
18.56575802, 4.89996852, 21.02429154, 13.6657895
18.62430693, 4.89997223, 21.11436108, 13.7243347
16.54216482, 4.89997511, 17.91106109, 11.64218971
18.71146936, 4.89997747, 21.24844907, 13.81149189
18.13316504, 4.89997923, 20.35874741, 13.23318581
18.77330006, 4.89998067, 21.34356829, 13.87331939
Despite the differences in torque command (a_cmd) I still get similar performance, in that the arm drives to the right position fairly quickly. As another experiment I set the initial position to pi/2 and just fed back the gravity term to the torque output. My understanding of this is that the arm should float, ala a gravity compensation sort of thing. But it just drops as if a small torque is applied. Thanks again!
