3. Tutorial 1: How to simulate the motion dynamics

This tutorial explains the steps required to simulate the robot motion dynamics for a 2-joint dynamic robot model. I assume you have setup this package and the respective dependencies, see Installation.

3.1. Required Steps

To use the simulator do the following steps:

  • Import the simulator

    Import the DynamicsSimulator class, which is provided by each of the dynsim modules (one for each robot model / setup). This class interconntects the ODE solver SUNDIALS IDA, one of the robot models via the lwrsundialsmodels library and Python. It does most of the setup of the ODE solver and provides robot-specific functions to simulate and to get the results.

    So for our example we will use the model of joints 2 and 4 of the robot, lwr4_q2q4. The import then looks as follows:

    from lwrsim.lwr4_q2q4 import DynamicsSimulator
    

    Currently, you can also import from lwr4 and lwr4_q1q2q4 accordingly. See the lwrmodels package.

    Note

    If you get an error that the module dynsim has not been found, i.e. ImportError: No module named dynsim, make sure you are _not_ in the source directory of this package or in any other directory with a lwrsim subfolder.

    Note

    If you get an error such as ImportError: /usr/lib64/libsundials_kinsol.so.1: undefined symbol: dgbtrs_ add ‘lapack’ to the libraries list in the setup.py of python-sundials. Then remove sundials/sundials.c, remove the build directory and call python3 setup.py install again.

  • Define the solver settings

    Most of the settings for the ODE solver are already made in the DynamicsSimulator class. So you only have to set the sampling time, inital state of the robot and the tolerances. This should look as follows:

    # Get number of joints of the robot
    nRob = DynamicsSimulator.numJointsRobot
    # Set the sampling time
    Ts = 0.01
    # Set inital joint positions
    q0 = [0.0, 3.0, 0.0, -1.5, 0.0, 0.0, 0.0]
    # Set inital joint velocities
    dq0 = [0.0]*nRob
    # Set absolute tolerance for the solver
    abstol = 1.0e-6
    # Set relative tolerance for the solver
    reltol = 1.0e-5
    

    Note

    Though the model is describing only two joints of the robot, the simulator expects and returns lists with values for all joints of the robot (here: 7). So in this case, we supply 7-element lists as initial joint positions and joint velocities but only the second and 4th elements are relevant. This applies to the torque input and the returned joint positions and velocities as well (see below).

  • Initialize the simulator

    With the chosen settings, you can now initialize an instance of DynamicsSimulator:

    # Now we can initialize the simulator / the ODE solver
    ds = DynamicsSimulator(sampleTime=Ts, q0=q0, dq0=dq0,
                            abstol=abstol, reltol=reltol)
    

    This completes the setup and we can now simulate.

  • Simulate

    On each call the solver simulates the robot motion to one point in time, i.e. for the chosen sampling time. For each call you have to supply a list of actuation torques, which are applied during the simulation. After each call you can get the list of joint positions and the list of joint velocities. Let us now simulate for 1000 samples, i.e. 10s. We do not actuate the robot for now, so we set the actuation torques to zero. The robot is therefore moving similar to an ordinary pendulum with two joints.

    # define a list to save the time to
    t = [0.0]
    # define a list to save the lists of joint positions to
    q = [q0]
    # define a list to save the lists of joint velocities to
    dq = [dq0]
    
    # Initialize list of actuation torques (all zero)
    tau = [0.0]*nRob
    
    # Let us simulate for 1000 samples, i.e. 1000*Ts = 10s
    for k in range(1000):
        # Simulate the robot motion to time T = T + Ts = k*Ts
        # with the supplied actuation torques tau
        ds.calc_next_state(tau)
    
        # Save the results
        t += [(k+1)*Ts]
        q += [ds.get_joint_positions()]
        dq += [ds.get_joint_velocities()]
    
  • Output the results

    If you have numpy and matplotlib installed, you can plot the results directly from Python:

    import numpy as np
    import pylab as P
    
    # First let us convert the data to numpy arrays
    t = np.array(t)
    q = np.array(q)
    dq = np.array(dq)
    
    # Then plot the data (refer to matplotlib documentation)
    P.subplot(2,1,1)
    P.plot(t, q[:,1], label='q2')
    P.plot(t, q[:,3], label='q4')
    P.legend()
    
    P.subplot(2,1,2)
    P.plot(t, dq[:,1], label='dq2')
    P.plot(t, dq[:,3], label='dq4')
    P.legend()
    
    P.show()
    

    This should look similar to this figure:

    _images/matplotlib2.png

    As one could expect the unactuated robot just hangs down, i.e. the joint positions converge to pi and zero ([odd times pi, even times pi] in genral).

    If you do not have numpy or matplotlib you can write the data to a file and use your favorite plot tool:

    # Assemble format string for values and the title line
    fmtstr = ', '.join(["{:7.3f}" for n in range(len(q0))])
    titlestr = ", ".join(["   t   "]
                    + ["   q{}  ".format(n+1) for n in range(len(q0))]
                    + ["  dq{}  ".format(n+1) for n in range(len(q0))])
    
    # Write the data to a file in Comma-Separated-Value format
    with open("/tmp/lwrsim_plotdata.csv", 'wt') as f:
        f.write(titlestr + "\n")
        for k in range(len(t)):
            f.write("{:7.3f}, ".format(t[k]))
            f.write(fmtstr.format(*q[k]))
            f.write(", ")
            f.write(fmtstr.format(*dq[k]))
            f.write("\n")
    

    To plot the generated data file with gnuplot for example do

    gnuplot -e "plot '/tmp/lwrsim_plotdata.csv' using 1:3 \
                     title 'q1' with lines, \
                     '/tmp/lwrsim_plotdata.csv' using 1:5 \
                     title 'q2' with lines" -persist
    

    This should look similar to:

    _images/gnuplot.png

3.2. Summary and Next Steps

We have seen how to simulate the motion of two joints without actuating them. You can now test different values for the initial positions and velocities to see how this affects the motion of the robot. Of course you can test the other models as well by adapting this example.

In the next tutorial we will implement a simple position controller to determine the actuation torques and simulate the closed loop motion.

You can also skip to the 3rd tutorial, which shows how to combine the dynamics simulation with fripy to emulate the interface of the real robot.

Table Of Contents

Previous topic

2. Reference

Next topic

4. Tutorial 2: Simulating the closed-loop motion dynamics

This Page