API  4.2
For MATLAB, Python, Java, and C++ users
exampleKinematicConstraints.py

This example demonstrates how Moco solves problems with kinematic constraints and includes a visualization of the Lagrange multipliers that Moco solves for.

1 # -------------------------------------------------------------------------- #
2 # OpenSim Moco: exampleKinematicConstraints.py #
3 # -------------------------------------------------------------------------- #
4 # Copyright (c) 2020 Stanford University and the Authors #
5 # #
6 # Author(s): Christopher Dembia #
7 # #
8 # Licensed under the Apache License, Version 2.0 (the "License"); you may #
9 # not use this file except in compliance with the License. You may obtain a #
10 # copy of the License at http://www.apache.org/licenses/LICENSE-2.0 #
11 # #
12 # Unless required by applicable law or agreed to in writing, software #
13 # distributed under the License is distributed on an "AS IS" BASIS, #
14 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. #
15 # See the License for the specific language governing permissions and #
16 # limitations under the License. #
17 # -------------------------------------------------------------------------- #
18 
19 # This example illustrates how Moco handles kinematic constraints with a model
20 # of a planar point mass with degrees of freedom tx and ty that is constrained
21 # to move along a parabola ty = tx^2. This example plots the trajectory of the
22 # point mass, which follows the parabola, and multiplies the kinematic
23 # constraint Jacobian and the Lagrange multipliers to visualize the constraint
24 # forces that Moco produces to ensure the point mass remains on the parabola.
25 # Lastly, this example computes the error in the equations of motion, showing
26 # how the constraint forces are necessary for satisfying the equations of
27 # motion.
28 #
29 # The constraint ty = tx^2 is represented internally as 0 = tx^2 - ty.
30 # The kinematic constraint Jacobian contains the partial derivative of
31 # each kinematic constraint (only one constraint in this case) with respect to
32 # each degree of freedom:
33 #
34 # G = [2*tx, -1]
35 #
36 # This example is related to an explanation of kinematic constraints in the
37 # appendix of the Moco paper.
38 
39 import os
40 import opensim as osim
41 import numpy as np
42 
43 model = osim.Model()
44 model.setName('planar_point_mass')
45 g = abs(model.get_gravity()[1])
46 
47 # We use an intermediate massless body for the X translation degree of freedom.
48 massless = osim.Body('massless', 0, osim.Vec3(0), osim.Inertia(0))
49 model.addBody(massless);
50 mass = 1.0
51 body = osim.Body('body', mass, osim.Vec3(0), osim.Inertia(0))
52 model.addBody(body)
53 
54 body.attachGeometry(osim.Sphere(0.05))
55 
56 jointX = osim.SliderJoint('tx', model.getGround(), massless)
57 coordX = jointX.updCoordinate(osim.SliderJoint.Coord_TranslationX)
58 coordX.setName('tx')
59 model.addJoint(jointX)
60 
61 # The joint's x axis must point in the global "+y" direction.
62 jointY = osim.SliderJoint('ty',
63  massless, osim.Vec3(0), osim.Vec3(0, 0, 0.5 * np.pi),
64  body, osim.Vec3(0), osim.Vec3(0, 0, 0.5 * np.pi))
65 coordY = jointY.updCoordinate(osim.SliderJoint.Coord_TranslationX)
66 coordY.setName('ty')
67 model.addJoint(jointY)
68 
69 # Add the kinematic constraint ty = tx^2.
70 constraint = osim.CoordinateCouplerConstraint()
71 independentCoords = osim.ArrayStr()
72 independentCoords.append('tx')
73 constraint.setIndependentCoordinateNames(independentCoords)
74 constraint.setDependentCoordinateName('ty')
75 coefficients = osim.Vector(3, 0) # 3 elements initialized to 0.
76 # The polynomial is c0*tx^2 + c1*tx + c2; set c0 = 1, c1 = c2 = 0.
77 coefficients.set(0, 1)
78 constraint.setFunction(osim.PolynomialFunction(coefficients))
79 model.addConstraint(constraint)
80 
81 study = osim.MocoStudy()
82 problem = study.updProblem()
83 problem.setModel(model)
84 
85 phase0 = problem.getPhase(0)
86 # Setting stricter bounds for the speeds improves convergence.
87 phase0.setDefaultSpeedBounds(osim.MocoBounds(-5, 5))
88 
89 problem.setTimeBounds(0, 0.8)
90 # Start the motion at (-1, 1).
91 problem.setStateInfo('/jointset/tx/tx/value', [-2, 2], -1.0)
92 problem.setStateInfo('/jointset/ty/ty/value', [-2, 2], 1.0)
93 
94 solver = study.initCasADiSolver()
95 solution = study.solve()
96 
97 # If matplotlib is installed, plot the trajectory of the mass and the
98 # constraint force applied to the mass throughout the motion.
99 plot = False
100 # The following environment variable is set during automated testing.
101 if os.getenv('OPENSIM_USE_VISUALIZER') != '0':
102  try:
103  import pylab as pl
104  from scipy.interpolate import InterpolatedUnivariateSpline
105  plot = True
106  except:
107  print('Skipping plotting')
108 
109 if plot:
110 
111  # Create a figure.
112  fig = pl.figure()
113  ax = fig.add_subplot(1, 1, 1)
114  ax.set_xlabel('tx')
115  ax.set_ylabel('ty')
116 
117  # Extract the trajectory from the solution.
118  time = solution.getTimeMat()
119  tx = solution.getStateMat('/jointset/tx/tx/value')
120  ty = solution.getStateMat('/jointset/ty/ty/value')
121  multiplier = -solution.getMultiplierMat(solution.getMultiplierNames()[0])
122  print('Number of multipliers: %i' % len(solution.getMultiplierNames()))
123 
124  # Plot the trajectory.
125  ax.set_aspect('equal')
126  pl.plot(tx, ty, color='black')
127 
128  # Compute generalized accelerations from the solution trajectory, for use
129  # in computing the residual of the equations of motion.
130  txSpline = InterpolatedUnivariateSpline(time, tx)
131  # Evaluate the second derivative of the spline.
132  accelx = txSpline(time, 2)
133  tySpline = InterpolatedUnivariateSpline(time, ty)
134  accely = tySpline(time, 2)
135 
136  # Loop through the trajectory and compute the constraint force.
137  model.initSystem()
138  statesTraj = solution.exportToStatesTrajectory(model)
139  matter = model.getMatterSubsystem()
140  constraintJacobian = osim.Matrix()
141  itime = 0
142  while itime < statesTraj.getSize():
143  state = statesTraj.get(itime)
144  model.realizePosition(state)
145  # Calculate the position-level constraint Jacobian, 2 x 1 (number of
146  # degrees of freedom by number of constraints).
147  # Pq = [2 * tx, -1]
148  matter.calcPq(state, constraintJacobian)
149  jac = np.array([constraintJacobian.get(0, 0),
150  constraintJacobian.get(1, 0)])
151  # The constraint generates generalized forces G^T * lambda.
152  constraintForces = jac * multiplier[itime]
153  # Scale down the force vector so it can be easily visualized with the
154  # point mass trajectory.
155  constraintForcesViz = 0.010 * constraintForces
156  pl.arrow(tx[itime], ty[itime], constraintForcesViz[0],
157  constraintForcesViz[1], width=0.005, color='blue')
158 
159  # Residual = F - ma
160  residualx = +constraintForces[0] - mass * accelx[itime]
161  residualy = -mass * g + constraintForces[1] - mass * accely[itime]
162  print(
163  'time %f: residuals: %f, %f' % (time[itime], residualx, residualy))
164 
165  # We plot the constraint force for every 10th time point in the solution
166  # to avoid cluttering the plot.
167  itime += 10
168 
169  pl.show()
170 
171