Update to use full equations of motion #5

brocksam · 2023-08-24T11:06:48Z

No description provided.

src/generate_eom.py

moorepants · 2023-08-24T18:11:29Z

Might be worth a try to increase the sympy cache SYMPY_CACHE_SIZE=2000.

moorepants · 2023-08-24T18:50:26Z

I was trying to run this but am getting:

---------------------------------------------------------------------------
TypeError                                 Traceback (most recent call last)
File ~/Manuscripts/muscle-driven-bicycle-paper/src/test_jac.py:4
      1 from generate_eom import gen_eom_for_opty
      2 from utils import csejac
----> 4 data = gen_eom_for_opty()
      6 csejac(data.eom, data.x)
      8 data.eom.jacobian(data.x)

File ~/Manuscripts/muscle-driven-bicycle-paper/src/generate_eom.py:534, in gen_eom_for_opty(steer_with, include_roll_torque)
    531 tau_a, tau_d, b_tanh = sm.symbols('tau_a, tau_d, b_tanh')
    533 bicep_right_pathway = mec.LinearPathway(gm, hm)
--> 534 bicep_right_activation = FirstOrderActivationDeGroote2016.with_defaults(
    535     'bi_r',
    536     activation_time_constant=tau_a,
    537     deactivation_time_constant=tau_d,
    538     smoothing_rate=b_tanh,
    539 )
    540 bicep_right = MusculotendonDeGroote2016(
    541     'bi_r',
    542     bicep_right_pathway,
   (...)
    549     fiber_damping_coefficient=beta,
    550 )
    552 bicep_left_pathway = mec.LinearPathway(im, jm)

TypeError: FirstOrderActivationDeGroote2016.with_defaults() got an unexpected keyword argument 'activation_time_constant'

brocksam · 2023-08-24T19:19:13Z

I was trying to run this but am getting:


---------------------------------------------------------------------------

TypeError                                 Traceback (most recent call last)

File ~/Manuscripts/muscle-driven-bicycle-paper/src/test_jac.py:4

      1 from generate_eom import gen_eom_for_opty

      2 from utils import csejac

----> 4 data = gen_eom_for_opty()

      6 csejac(data.eom, data.x)

      8 data.eom.jacobian(data.x)



File ~/Manuscripts/muscle-driven-bicycle-paper/src/generate_eom.py:534, in gen_eom_for_opty(steer_with, include_roll_torque)

    531 tau_a, tau_d, b_tanh = sm.symbols('tau_a, tau_d, b_tanh')

    533 bicep_right_pathway = mec.LinearPathway(gm, hm)

--> 534 bicep_right_activation = FirstOrderActivationDeGroote2016.with_defaults(

    535     'bi_r',

    536     activation_time_constant=tau_a,

    537     deactivation_time_constant=tau_d,

    538     smoothing_rate=b_tanh,

    539 )

    540 bicep_right = MusculotendonDeGroote2016(

    541     'bi_r',

    542     bicep_right_pathway,

   (...)

    549     fiber_damping_coefficient=beta,

    550 )

    552 bicep_left_pathway = mec.LinearPathway(im, jm)



TypeError: FirstOrderActivationDeGroote2016.with_defaults() got an unexpected keyword argument 'activation_time_constant'

Should just be using the normal __init__, not with_constants constructor.

brocksam · 2023-08-24T19:35:03Z

@moorepants I just pushed commit with the fix so you should now be able to generate the EoMs with the muscles.

brocksam · 2023-08-25T00:52:59Z

Using the branch from this opty PR csu-hmc/opty#102 we can now get through the Jacobian derivation and compilation. It takes about 3 minutes on my laptop to get through everything and hit Ipopt. Sadly Ipopt immediate fails with the message EXIT: Invalid number in NLP function or derivative detected.

moorepants · 2023-08-25T07:24:08Z

Did you manually check prob.con(initial_guess) and prob.jac(initial_guess)?

moorepants · 2023-08-25T07:26:31Z

Also are you testing the jacobian's without the muscles? So only steer torque?

brocksam · 2023-08-25T07:32:20Z

Did you manually check prob.con(initial_guess) and prob.jac(initial_guess)?

No, but this sounds like a good suggestion of where to start investigating this morning, thanks. Especially as we're currently using an array of zeros for the initial guess, could easily be a floating point division by zero or similar somewhere. Will also look directly at the generated C code.

Also are you testing the jacobian's without the muscles? So only steer torque?

I've tried both, both immediate kill Ipopt. So that likely means is something common is a term associated with the EoMs. I'm starting to investigate with steer torque only.

moorepants · 2023-08-25T07:56:55Z

The error seems to be in the objective:

EXIT: Invalid number in NLP function or derivative detected.
---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
File ~/Manuscripts/muscle-driven-bicycle-paper/src/opty_ocp.py:152
    149 initial_guess = np.zeros(problem.num_free)
    151 # Find the optimal solution.
--> 152 sol, info = problem.solve(initial_guess)
    154 q1_sol = sol[:NUM_NODES]
    155 q2_sol = sol[NUM_NODES:2*NUM_NODES]

File ~/miniconda/envs/muscle-bike-paper/lib/python3.11/site-packages/cyipopt/cython/ipopt_wrapper.pyx:642, in ipopt_wrapper.Problem.solve()

File ~/miniconda/envs/muscle-bike-paper/lib/python3.11/site-packages/cyipopt/cython/ipopt_wrapper.pyx:700, in ipopt_wrapper.gradient_cb()

File ~/src/opty/opty/direct_collocation.py:226, in Problem.gradient(self, free)
    203 """Returns the value of the gradient of the objective function given a
    204 solution to the problem.
    205 
   (...)
    223 
    224 """
    225 # This should return a column vector.
--> 226 return self.obj_grad(free)

File ~/Manuscripts/muscle-driven-bicycle-paper/src/opty_ocp.py:39, in obj_grad(free)
     37 q2 = free[NUM_NODES:2*NUM_NODES]
     38 grad = np.zeros_like(free)
---> 39 dJdq1 = np.pi*LATERAL_DISPLACEMENT*(LATERAL_DISPLACEMENT*(1 - cos(np.pi*q1/LONGITUDINAL_DISPLACEMENT))/2 - q2)*sin(pi*q1/LONGITUDINAL_DISPLACEMENT)/LONGITUDINAL_DISPLACEMENT
     40 dJdq2 = LATERAL_DISPLACEMENT*(cos(pi*q1/LONGITUDINAL_DISPLACEMENT) - 1) + 2*q2
     41 grad[:NUM_NODES] = dJdq1

NameError: name 'cos' is not defined
> /home/moorepants/Manuscripts/muscle-driven-bicycle-paper/src/opty_ocp.py(39)obj_grad()
     37     q2 = free[NUM_NODES:2*NUM_NODES]
     38     grad = np.zeros_like(free)
---> 39     dJdq1 = np.pi*LATERAL_DISPLACEMENT*(LATERAL_DISPLACEMENT*(1 - cos(np.pi*q1/LONGITUDINAL_DISPLACEMENT))/2 - q2)*sin(pi*q1/LONGITUDINAL_DISPLACEMENT)/LONGITUDINAL_DISPLACEMENT
     40     dJdq2 = LATERAL_DISPLACEMENT*(cos(pi*q1/LONGITUDINAL_DISPLACEMENT) - 1) + 2*q2
     41     grad[:NUM_NODES] = dJdq1

moorepants · 2023-08-25T07:59:06Z

The constraints and jacobian seem to evaluate:

In [2]: problem.con(initial_guess)
Out[2]: array([0., 0., 0., ..., 0., 0., 0.])
In [4]: problem.jacobian(initial_guess)
Out[4]: array([ 0.5,  0. ,  0. , ...,  1. , -1. ,  1. ])

moorepants · 2023-08-25T07:59:47Z

Also not bad speed for 10k lines of c code:

In [5]: %timeit problem.jacobian(initial_guess)
2.03 ms ± 116 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)

brocksam · 2023-08-25T08:11:04Z

The error seems to be in the objective:

EXIT: Invalid number in NLP function or derivative detected.
---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
File ~/Manuscripts/muscle-driven-bicycle-paper/src/opty_ocp.py:152
    149 initial_guess = np.zeros(problem.num_free)
    151 # Find the optimal solution.
--> 152 sol, info = problem.solve(initial_guess)
    154 q1_sol = sol[:NUM_NODES]
    155 q2_sol = sol[NUM_NODES:2*NUM_NODES]

File ~/miniconda/envs/muscle-bike-paper/lib/python3.11/site-packages/cyipopt/cython/ipopt_wrapper.pyx:642, in ipopt_wrapper.Problem.solve()

File ~/miniconda/envs/muscle-bike-paper/lib/python3.11/site-packages/cyipopt/cython/ipopt_wrapper.pyx:700, in ipopt_wrapper.gradient_cb()

File ~/src/opty/opty/direct_collocation.py:226, in Problem.gradient(self, free)
    203 """Returns the value of the gradient of the objective function given a
    204 solution to the problem.
    205 
   (...)
    223 
    224 """
    225 # This should return a column vector.
--> 226 return self.obj_grad(free)

File ~/Manuscripts/muscle-driven-bicycle-paper/src/opty_ocp.py:39, in obj_grad(free)
     37 q2 = free[NUM_NODES:2*NUM_NODES]
     38 grad = np.zeros_like(free)
---> 39 dJdq1 = np.pi*LATERAL_DISPLACEMENT*(LATERAL_DISPLACEMENT*(1 - cos(np.pi*q1/LONGITUDINAL_DISPLACEMENT))/2 - q2)*sin(pi*q1/LONGITUDINAL_DISPLACEMENT)/LONGITUDINAL_DISPLACEMENT
     40 dJdq2 = LATERAL_DISPLACEMENT*(cos(pi*q1/LONGITUDINAL_DISPLACEMENT) - 1) + 2*q2
     41 grad[:NUM_NODES] = dJdq1

NameError: name 'cos' is not defined
> /home/moorepants/Manuscripts/muscle-driven-bicycle-paper/src/opty_ocp.py(39)obj_grad()
     37     q2 = free[NUM_NODES:2*NUM_NODES]
     38     grad = np.zeros_like(free)
---> 39     dJdq1 = np.pi*LATERAL_DISPLACEMENT*(LATERAL_DISPLACEMENT*(1 - cos(np.pi*q1/LONGITUDINAL_DISPLACEMENT))/2 - q2)*sin(pi*q1/LONGITUDINAL_DISPLACEMENT)/LONGITUDINAL_DISPLACEMENT
     40     dJdq2 = LATERAL_DISPLACEMENT*(cos(pi*q1/LONGITUDINAL_DISPLACEMENT) - 1) + 2*q2
     41     grad[:NUM_NODES] = dJdq1

Have you pulled the latest commits? I should have fixed this with 7b95822 last night.

brocksam · 2023-08-25T08:20:03Z

Both problem.con(initial_guess) and problem.jacobian(initial_guess) produce NaN entries if an initial guess of all zeros is used. OCP runs with a non-zero initial guess. Good news. Ipopt quick starts doing restoration steps and then exceed max iterations, but that's to be expected with such a terrible initial guess. I'll work on producing a better one and hopefully we get somewhere.

moorepants · 2023-08-31T15:06:08Z

Can you push the latest up for this? There were some edits we made together that aren't yet there.

brocksam · 2023-09-01T08:41:18Z

Can you push the latest up for this? There were some edits we made together that aren't yet there.

Done.

…onstraints to using them directly.

Switch from using the derivatives of the holonomic and nonholonomic constraints to using them directly.

Update to use full equations of motion

e542053

brocksam force-pushed the opty-ocp branch from 3c119e3 to e542053 Compare August 24, 2023 11:07

moorepants reviewed Aug 24, 2023

View reviewed changes

src/generate_eom.py Outdated Show resolved Hide resolved

moorepants reviewed Aug 24, 2023

View reviewed changes

src/generate_eom.py Outdated Show resolved Hide resolved

brocksam added 3 commits August 24, 2023 13:46

Ensure x sorted in order for ease

c365511

Use full q and u in x

a54759b

Add first go at trajectory tracking OCP with opty

9dc17c9

brocksam added 3 commits August 24, 2023 21:32

Remove incorrect with_default_constants constructor for musculotendons

773319c

Fix bug in including torque with include_roll_torques

f58eea1

Improve compatibility in formulating OCP with SteerWith members

9dd6a2d

brocksam added 3 commits August 25, 2023 02:45

Add missing np. module prefix to NumPy functions

7b95822

Add bounds for all inputs

4a8a259

Remove Sam-specific tmp_dir argument

a9eac4d

brocksam added 4 commits August 25, 2023 11:24

Refactor containers, add function for generating initial guess

6ffa434

Refactor containers, add function for generating initial guess

18e6bef

Add constants field to Metadata container

9eefff9

Fix typo in signatures, add perturbation to all guesses

8d9ac48

brocksam added 4 commits September 1, 2023 09:39

Adjust instance constraints

f8c7510

Improve bounds

097e9d7

Use gradient scaling on Ipopt config

f9fc624

Add missing constants to Metadata instance

d609df0

moorepants added 4 commits September 1, 2023 11:10

Switch from using the derivatives of the holonomic and nonholonomic c…

3d72e02

…onstraints to using them directly.

Added opty dependencies.

fe473a3

Added kindiff eqs into the EoMs, now it runs.

41b65b0

Merge pull request #6 from moorepants/use-constraints

7f297f6

Switch from using the derivatives of the holonomic and nonholonomic constraints to using them directly.

moorepants merged commit e34a8fd into main Sep 1, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Update to use full equations of motion #5

Update to use full equations of motion #5

brocksam commented Aug 24, 2023

moorepants commented Aug 24, 2023

moorepants commented Aug 24, 2023

brocksam commented Aug 24, 2023

brocksam commented Aug 24, 2023

brocksam commented Aug 25, 2023

moorepants commented Aug 25, 2023

moorepants commented Aug 25, 2023

brocksam commented Aug 25, 2023

moorepants commented Aug 25, 2023

moorepants commented Aug 25, 2023

moorepants commented Aug 25, 2023

brocksam commented Aug 25, 2023

brocksam commented Aug 25, 2023

moorepants commented Aug 31, 2023

brocksam commented Sep 1, 2023

Update to use full equations of motion #5

Update to use full equations of motion #5

Conversation

brocksam commented Aug 24, 2023

moorepants commented Aug 24, 2023

moorepants commented Aug 24, 2023

brocksam commented Aug 24, 2023

brocksam commented Aug 24, 2023

brocksam commented Aug 25, 2023

moorepants commented Aug 25, 2023

moorepants commented Aug 25, 2023

brocksam commented Aug 25, 2023

moorepants commented Aug 25, 2023

moorepants commented Aug 25, 2023

moorepants commented Aug 25, 2023

brocksam commented Aug 25, 2023

brocksam commented Aug 25, 2023

moorepants commented Aug 31, 2023

brocksam commented Sep 1, 2023