Thread Subject: Simulation of Hybrid Dynamics

I'm looking for guidelines on how to successfully create a simulation of hybrid dynamics, i.e. continuous dynamics + discrete impact events. Specifically I'm working on biped models, starting with a simple compass gait biped.

So far, my simulation loop looks like this (in pseudocode to make it concise):

while not finished or stop_sim
   run ode45 until impact (or t_end)
   if Impact
      calculate impact and set new "initial conditions"
      (check if some constraints were "released" when impact occurred)
   end
end


Inside my derivative func:
   get H, h, A and u (Dynamics, constraints and control)
   calculate Lambda and derivative (q_dot_dot)


After each step I run an output func:
   check constraints and update Configuration (variable vector holding constraints status)


The check_constraints function is where I think my problems are because the simulation behaves in unexpected ways. Inside the func:
   if constraint_status == 0
      look for impact (when Leg_y<=Floor and Vel_y<0)
   else
      look for detachment (when Lambda_y<=0)
   end


That's the grand scheme of things. The m file actually has about 500 lines.

I'd like to know if my framework is correct or if you'd recommend some changes to it. Right now the simulation seems to work ok in some cases but works terribly bad in others. For example: One leg impacts the ground and "passes through" for a bit until bouncing back (as if the constraints where elastic, which they aren't). Another example: The leg impacts the ground and then gets detached from it even though Lambda_y is still positive, i.e. the ground is still "pushing" the leg.

I'm betting all my money on a constraints issue but so far I haven't been able to figure them out. My constraints for a pivot leg look like:
Const. 1: x - x_0 = 0
Const. 2: y - y_0 >= 0

This is my opinion: think the specifics of your code seem right. Essentially you are trying to switch betwen two regimes based on some conditions and want to preserve the initial conditions at the boundary.

In my own experience, there have been two approaches that I have taken:

1. MATLAB-based approach: Using the ode45 and adjusting the t_end till you hit a condition. There was a lot of bookkeeping that I had to do because ode45 does not have the ability to conditionally execute based on other conditional code. But my dynamic equations were simple and being in graduate school, it was a good exercise to go through.

2. Simulink-based: Over the last couple of years, I have found that the solver in Simulink is lot more powerful in trying to execute conditional statements(in block form) and readjust the solution of the system. Here is an example of a demo:(I am sure there are others)

http://www.mathworks.com/matlabcentral/fileexchange/28196-robot-soccer-an-exercise-in-learning-the-key-features-of-simulink

where I switch between different regimes(collisions) and I do absolutely no bookkeeping for the solver. Observe how I incorporate different contact equations in the Embedded MATLAB function block. The solver chooses its steps based on what attributes I specified.

I like the design pattern that I see here for these switching dynamics kinds of problems where the differential equations of the main system is not changing (different co-efficients for example). If that happened, then I would use triggered/enabled subsystem semantics to incorporate that.

Uhhh... why are you not using the events option in ODE45?
_____________From help___________
[T,Y,TE,YE,IE] = solver(odefun,tspan,y0,options) solves as above while also finding where functions of , called event functions, are zero. For each event function, you specify whether the integration is to terminate at a zero and whether the direction of the zero crossing matters. Do this by setting the 'Events' property to a function, e.g., events or @events, and creating a function [value,isterminal,direction] = events(t,y). For the ith event function in events,

    *

      value(i) is the value of the function.
    *

      isterminal(i) = 1, if the integration is to terminate at a zero of this event function and 0 otherwise.
    *

      direction(i) = 0 if all zeros are to be computed (the default), +1 if only the zeros where the event function increases, and -1 if only the zeros where the event function decreases.

Corresponding entries in TE, YE, and IE return, respectively, the time at which an event occurs, the solution at the time of the event, and the index i of the event function that vanishes.

Thanks for your responses!
Saurabh, I tried to open your simulation but some errors came up. It is good to know however that I can use embedded functions.


"Joel" <esposito@usna.edu> wrote in message <i5n2o8$d0p$1@fred.mathworks.com>...
> Uhhh... why are you not using the events option in ODE45?

I was using the 'Events' function but it's called whenever derivative is called. This had 2 problems:
1. It's called about 6 times per step
2. Some times impact will occur for a large delta_t (step size) but not for a smaller one. ODE45 then decides which step size to use for the step but the event was already registered by my function (messing with the constraints status variable).

As I said, I'm using the Output function which is called once after each integration step. Now I understand that it might cause some precision error but could it be the source of all the strange behavior?


> _____________From help___________
> [T,Y,TE,YE,IE] = solver(odefun,tspan,y0,options) ...
> Corresponding entries in TE, YE, and IE return, respectively, the time at which an event occurs, the solution at the time of the event, and the index i of the event function that vanishes.

Maybe I should stop the simulation for every event (impact or detachment) and use those extra parameters to check what happened and update the next initial conditions.

Thanks for your responses!
Saurabh, I tried to open your simulation but some errors came up. It is good to know however that I can use embedded functions.


"Joel" <esposito@usna.edu> wrote in message <i5n2o8$d0p$1@fred.mathworks.com>...
> Uhhh... why are you not using the events option in ODE45?

I was using the 'Events' function but it's called whenever derivative is called. This had 2 problems:
1. It's called about 6 times per step
2. Some times impact will occur for a large delta_t (step size) but not for a smaller one. ODE45 then decides which step size to use for the step but the event was already registered by my function (messing with the constraints status variable).

As I said, I'm using the Output function which is called once after each integration step. Now I understand that it might cause some precision error but could it be the source of all the strange behavior?


> _____________From help___________
> [T,Y,TE,YE,IE] = solver(odefun,tspan,y0,options) ...
> Corresponding entries in TE, YE, and IE return, respectively, the time at which an event occurs, the solution at the time of the event, and the index i of the event function that vanishes.

Maybe I should stop the simulation for every event (impact or detachment) and use those extra parameters to check what happened and update the next initial conditions.

Well, I've worked the past few days on "migrating" from the Output function and into the Event function but the errors still remain... I think I'll have to use different dynamics for different configurations (one foot on floor, both feet on floor, both feet on air)

"Jonathan Spitz" <lothas@gmail.com> wrote in message <i5nb4e$adu$1@fred.mathworks.com>...
> Thanks for your responses!
> Saurabh, I tried to open your simulation but some errors came up. It is good to know however that I can use embedded functions.
>
>
> "Joel" <esposito@usna.edu> wrote in message <i5n2o8$d0p$1@fred.mathworks.com>...
> > Uhhh... why are you not using the events option in ODE45?
>
> I was using the 'Events' function but it's called whenever derivative is called. This had 2 problems:
> 1. It's called about 6 times per step
> 2. Some times impact will occur for a large delta_t (step size) but not for a smaller one. ODE45 then decides which step size to use for the step but the event was already registered by my function (messing with the constraints status variable).
>
> As I said, I'm using the Output function which is called once after each integration step. Now I understand that it might cause some precision error but could it be the source of all the strange behavior?
>
>
> > _____________From help___________
> > [T,Y,TE,YE,IE] = solver(odefun,tspan,y0,options) ...
> > Corresponding entries in TE, YE, and IE return, respectively, the time at which an event occurs, the solution at the time of the event, and the index i of the event function that vanishes.
>
> Maybe I should stop the simulation for every event (impact or detachment) and use those extra parameters to check what happened and update the next initial conditions.