API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 20th, 2017
97
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.40 KB | None | 0 0
raw download clone embed print report
  1. with env:
  2. robot = env.GetRobots()[0]
  3. robot.GetLinks()[0].SetStatic(True)
  4. env.StopSimulation()
  5. env.StartSimulation(timestep=0.0001)
  6.  
  7. dt = 0.001
  8. w = 100
  9. eta = 5
  10. Kp = [w*w]
  11. Kv = [2*eta*w]
  12. # Desired pos, vel and acc
  13. cmd_p = [3.14/2]
  14. cmd_v = [0]
  15. cmd_a = [0]
  16.  
  17. while True:
  18. with env:
  19. torqueconfiguration, torquecoriolis, torquegravity = robot.ComputeInverseDynamics([1],None,returncomponents=True)
  20. err_p = cmd_p - robot.GetDOFValues()
  21. err_v = cmd_v - robot.GetDOFVelocities()
  22.  
  23. # ID Controller
  24. M = compute_inertia_matrix(robot, robot.GetDOFValues())
  25. a_cmd = (Kp*err_p + Kv*err_v + cmd_a)
  26. taus = torquegravity + torquecoriolis + M.dot(a_cmd.transpose()).transpose()
  27.  
  28. # Just PD(ish) controller
  29. #taus = Kp*err_p - Kv*robot.GetDOFVelocities()
  30.  
  31. with robot:
  32. robot.SetDOFTorques(taus,False) # True = use limits
  33. print (taus, torquegravity+torquecoriolis, a_cmd, M.dot(a_cmd.transpose()).transpose())
  34. time.sleep(dt)
  35.  
  36.  
  37. # https://scaron.info/teaching/equations-of-motion.html
  38. def compute_inertia_matrix(robot, q, external_torque=None):
  39. n = len(q)
  40. M = np.zeros((n, n))
  41. with robot:
  42. robot.SetDOFValues(q)
  43. for (i, e_i) in enumerate(np.eye(n)):
  44. m, c, g = robot.ComputeInverseDynamics(e_i, external_torque, returncomponents=True)
  45. M[:, i] = m
  46. return M
  47.  
  48.  
  49.  
  50. <?xml version="1.0" encoding="utf-8"?>
  51. <Robot name="Pendulum">
  52. <RotationAxis>0 1 0 90</RotationAxis> <!-- makes the pendulum vertical -->
  53. <KinBody>
  54. <!-- <Mass type="mimicgeom"><density>100000</density></Mass> -->
  55. <Body name="Base" type="dynamic">
  56. <Translation>0.0 0.0 0.0</Translation>
  57. <Geom type="cylinder">
  58. <rotationaxis>1 0 0 90</rotationaxis>
  59. <radius>0.3</radius>
  60. <height>0.02</height>
  61. <ambientColor>1. 0. 0.</ambientColor>
  62. <diffuseColor>1. 0. 0.</diffuseColor>
  63. </Geom>
  64. <mass type="custom">
  65. <!-- specify the total mass-->
  66. <total>5.0</total>
  67. <!-- specify the 3x3 inertia matrix-->
  68. <!--<inertia>2 0 0 0 3 0 0 0 5</inertia> -->
  69. <!-- specify the center of mass (if using ODE physics engine, should be 0)-->
  70. <com>0.1 0.0 0.0</com>
  71. </mass>
  72. </Body>
  73. <Body name="Arm0" type="dynamic">
  74. <offsetfrom>Base</offsetfrom>
  75. <!-- translation and rotation will be relative to Base -->
  76. <Translation>0 0 0</Translation>
  77. <Geom type="box">
  78. <Translation>1 0 0</Translation>
  79. <Extents>1 0.1 0.1</Extents>
  80. <ambientColor>1. 0. 0.</ambientColor>
  81. <diffuseColor>1. 0. 0.</diffuseColor>
  82. </Geom>
  83. <mass type="custom">
  84. <!-- specify the total mass-->
  85. <total>1.0</total>
  86. <!-- specify the 3x3 inertia matrix-->
  87. <!--<inertia>2 0 0 0 3 0 0 0 5</inertia> -->
  88. <!-- specify the center of mass (if using ODE physics engine, should be 0)-->
  89. <com>0.5 0.0 0.0</com>
  90. </mass>
  91. </Body>
  92. <Joint circular="true" name="Joint0" type="hinge">
  93. <Body>Base</Body>
  94. <Body>Arm0</Body>
  95. <offsetfrom>Arm0</offsetfrom>
  96. <weight>0</weight>
  97. <axis>0 0 1</axis>
  98. <maxvel>100</maxvel>
  99. <resolution>1</resolution>
  100. </Joint>
  101. </KinBody>
  102. </Robot>
  103.  
  104. taus, torquegravity+torquecoriolis, a_cmd, M*a_cmd
  105. 3464.88331508, 0.48809828, 5329.83879509, 3464.39521681
  106. 330.67177959, 1.47549936, 506.45581573, 329.19628023
  107. -785.91806527, 2.45531014, -1212.88211601, -788.37337541
  108. -1065.4689484, 3.23603844, -1644.16151823, -1068.70498685
  109. -1027.47479809, 3.80261774, -1586.58063974, -1031.27741583
  110. -877.83110127, 4.18635604, -1356.94993433, -882.01745731
  111. -707.25108627, 4.4371714, -1094.9050118, -711.68825767
  112. -554.34483533, 4.6006198, -859.91608481, -558.94545512
  113. -432.22314217, 4.70818921, -672.20204828, -436.93133138
  114. -327.797496, 4.7768792, -511.65288492, -332.5743752
  115. -240.77203429, 4.82021019, -377.83422228, -245.59224448
  116. -172.18942128, 4.84807059, -272.3653721, -177.03749186
  117. -117.58895761, 4.86591166, -188.39210657, -122.45486927
  118. -74.51920719, 4.87743369, -122.14867828, -79.39664088
  119. -39.91183436, 4.88473444, -68.91779816, -44.7965688
  120. -12.82321495, 4.88971433, -27.25066043, -17.71292928
  121. 8.45349476, 4.89281357, 5.47797105, 3.56068118
  122. 25.35468725, 4.89489884, 31.47659755, 20.45978841
  123. 38.84080509, 4.896309, 52.22230167, 33.94449609
  124. 48.72668147, 4.89724689, 67.42989936, 43.82943458
  125. 56.78552877, 4.89790152, 79.82711885, 51.88762725
  126. 65.515892, 4.89836756, 93.25772991, 60.61752444
  127. 68.81359264, 4.89867903, 98.33063633, 63.91491362
  128. 73.86961896, 4.89891052, 106.10878221, 68.97070844
  129. 76.67416578, 4.89907489, 110.42321674, 71.77509088
  130. 79.62549808, 4.89919702, 114.96354008, 74.72630105
  131. 85.17343708, 4.89928669, 123.49869291, 80.27415039
  132. 85.13686188, 4.89934963, 123.44232654, 80.23751225
  133. 85.75675034, 4.89939931, 124.39592466, 80.85735103
  134. 86.55192592, 4.89943807, 125.61921208, 81.65248785
  135. 86.39672231, 4.89946802, 125.38039121, 81.49725429
  136. 87.4299925, 4.89949202, 126.97000073, 82.53050048
  137. 87.42776523, 4.8995098, 126.96654682, 82.52825543
  138. 87.15472709, 4.8995251, 126.54646461, 82.255202
  139. 86.97240783, 4.89953825, 126.26595319, 82.07286958
  140. 86.98023044, 4.89954905, 126.27797137, 82.08068139
  141. 86.75364661, 4.89955809, 125.92936696, 81.85408852
  142. 86.9853716, 4.89956526, 126.28585591, 82.08580634
  143. 88.01679721, 4.89957062, 127.8726563, 83.1172266
  144. 89.2610231, 4.89957348, 129.78684557, 84.36144962
  145. 88.47969399, 4.89957495, 128.58479851, 83.58011903
  146. 88.77623594, 4.89957711, 129.04101359, 83.87665884
  147. 90.87280518, 4.89957739, 132.2665043, 85.9732278
  148. 88.9513552, 4.89957707, 129.3104279, 84.05177813
  149. 89.14100099, 4.89957773, 129.60218964, 84.24142327
  150.  
  151. taus, torquegravity+torquecoriolis, a_cmd, M*a_cmd
  152. -313.62240349, 0.98927261, -484.01796324, -314.61167611
  153. -242.03525463, 2.00886997, -375.45249938, -244.0441246
  154. -199.82226305, 2.79259699, -311.71516928, -202.61486003
  155. -190.02605484, 3.39367572, -297.56881625, -193.41973056
  156. -162.08293067, 3.8525617, -255.28537288, -165.93549237
  157. -125.84847045, 4.17559368, -200.03702174, -130.02406413
  158. -103.89936813, 4.40068949, -166.61547326, -108.30005762
  159. -82.32305905, 4.5566127, -133.66103347, -86.87967175
  160. -64.56801352, 4.66415211, -106.51102404, -69.23216563
  161. -49.68124446, 4.73812107, -83.72210081, -54.41936553
  162. -37.91265825, 4.78890663, -65.6947152, -42.70156488
  163. -27.99189838, 4.82374208, -50.48560071, -32.81564046
  164. -19.81225948, 4.84762415, -37.9382825, -24.65988362
  165. -12.55978349, 4.8636252, -26.80524414, -17.42340869
  166. -6.89165107, 4.87470983, -18.10209369, -11.7663609
  167. -3.13313345, 4.88256746, -12.33184754, -8.0157009
  168. 0.69831646, 4.88796162, -6.44560793, -4.18964516
  169. 3.86277859, 4.89166745, -1.58290594, -1.02888886
  170. 6.12163439, 4.8941598, 1.88842245, 1.22747459
  171. 8.58189707, 4.89593332, 5.67071346, 3.68596375
  172. 9.1580546, 4.89712981, 6.55526891, 4.26092479
  173. 11.81854706, 4.89798468, 10.64701905, 6.92056238
  174. 12.40540565, 4.89856409, 11.54898701, 7.50684156
  175. 14.04109075, 4.89897979, 14.06478609, 9.14211096
  176. 14.39924399, 4.89926951, 14.61534535, 9.49997448
  177. 14.98060951, 4.89947252, 15.50944153, 10.08113699
  178. 16.08890875, 4.89961544, 17.2142974, 11.18929331
  179. 16.01955973, 4.89971637, 17.10745133, 11.11984337
  180. 17.06493791, 4.89978831, 18.71561478, 12.16514961
  181. 17.35364328, 4.89983976, 19.15969772, 12.45380352
  182. 17.62239334, 4.89987688, 19.57310225, 12.72251646
  183. 17.84455913, 4.89990387, 19.91485424, 12.94465525
  184. 17.43825648, 4.89992362, 19.28974286, 12.53833286
  185. 17.58436934, 4.89993826, 19.51450935, 12.68443108
  186. 17.70571012, 4.8999492, 19.70117065, 12.80576093
  187. 18.40852272, 4.89995746, 20.78240808, 13.50856525
  188. 18.49492461, 4.89996372, 20.91532445, 13.59496089
  189. 18.56575802, 4.89996852, 21.02429154, 13.6657895
  190. 18.62430693, 4.89997223, 21.11436108, 13.7243347
  191. 16.54216482, 4.89997511, 17.91106109, 11.64218971
  192. 18.71146936, 4.89997747, 21.24844907, 13.81149189
  193. 18.13316504, 4.89997923, 20.35874741, 13.23318581
  194. 18.77330006, 4.89998067, 21.34356829, 13.87331939
  195.  
  196. time.sleep(dt)
  197.  
  198. env.StepSimulation(dt)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!