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.
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 DynamicsSimulatorCurrently, 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-5Note
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:
![]()
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" -persistThis should look similar to:
![]()
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.