Let us now extend the example from the previous tutorial to control the robot position. We will begin with a simple PD-Controller and then add a gravity compensation.
As in our previous tutorial, we do the imports and setup the simulator. In addition, we also import numpy, because we will use it later:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 | from lwrsim.lwr4_q2q4 import DynamicsSimulator
from lwrmodels.lwr4_q2q4.dynamics import get_gravity_torque_vector as tau_g
from sundials import IDAError
import numpy as np
from numpy import pi
# Get number of joints of the robot
nRob = DynamicsSimulator.numJointsRobot
# Set the sampling time
Ts = 0.01
# Set inital joint positions
q0 = [0.0, 0.5, 0.0, 0.0, 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
# Now we can initialize the simulator / the ODE solver
ds = DynamicsSimulator(sampleTime=Ts, q0=q0, dq0=dq0,
abstol=abstol, reltol=reltol)
|
Then define the settings for the controller and the simulation run:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 | # Determine how many samples to simulated for 10s simulation
nsamples = int(4.0 / Ts)
# Set the controller gains
# Using same setting for all joints here
Kp = np.ones((nRob,)) * 10.0
Ki = Kp * 0.0
Kd = 2.0 * np.sqrt(Kp) * 1.0
# Define the contant reference position
qref = np.array([0.0, 0.0, 0.0, 1.0*pi/2.0, 0.0, 0.0, 0.0])
# initialize an array to save the time to
t = np.zeros((nsamples, ))
# initialize an array to save the joint positions to
q = np.zeros((nsamples, nRob))
q[0,:] = q0[:]
# initialize an array to save the joint velocities to
dq = np.zeros((nsamples, nRob))
dq[0,:] = dq0[:]
# initialize an array to save the actuation torques to
tau = np.zeros((nsamples, nRob))
# initialize arrays for
# the last position error
lasterr = qref - q[0,:]
# the current position error
error = np.zeros((nRob,))
# the integral value of the error
errorint = np.zeros((nRob,))
# the derivative value of the error
derror = np.zeros((nRob,))
|
We can now simulate the robot motion. In contrast to the previous tutorial, we determine the joint actuation torques cur_tau for each simulation run according to a control law. As we want to use a position controller, we do the error computations for each run.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 | # Let us simulate for nsamples samples, i.e. 1000*Ts = 10s for Ts=0.01s
for k in range(1,nsamples):
# Compute errors etc.
error[:] = qref - q[k-1,:]
errorint += error
derror[:] = (error-lasterr)/Ts
lasterr[:] = error[:]
# Compute the torque from feedback control
cur_tau = (Kp * error
+ Kd * derror
+ Ki * errorint)
# Add gravity compensation torque
cur_tau[ds.simulatedJoints] += tau_g(q[k-1,ds.simulatedJoints])
try:
# Simulate the robot motion to time T = T + Ts = k*Ts
# with the supplied actuation torques tau
# Note that we convert the numpy array to a list
ds.calc_next_state(list(cur_tau))
except (IDAError):
print("Simulation failed, see error message above.")
break
# Save the results
t[k] = k*Ts
q[k,:] = ds.get_joint_positions()[:]
dq[k,:] = ds.get_joint_velocities()[:]
tau[k-1,:] = cur_tau[:]
tau[-1,:] = tau[-2,:]
|
As before, you can plot the results:
import pylab as P
# Plot the data (refer to matplotlib documentation)
P.subplot(3,1,1)
P.plot(t, q[:,1], 'b', label='q2')
P.plot(t[[0,-1]], [qref[1]]*2, 'b--', label='qref2')
P.plot(t, q[:,3], 'g', label='q4')
P.plot(t[[0,-1]], [qref[3]]*2, 'g--', label='qref4')
P.legend()
P.subplot(3,1,2)
P.plot(t, dq[:,1], label='dq2')
P.plot(t, dq[:,3], label='dq4')
P.legend()
P.subplot(3,1,3)
P.plot(t, tau[:,1], label='tau2')
P.plot(t, tau[:,3], label='tau4')
P.legend()
P.show()
# Assemble format string for values and the title line
fmtstr = ', '.join(["{:7.3f}" for n in range(nRob)])
titlestr = ", ".join([" t "]
+ [" q{} ".format(n+1) for n in range(nRob)]
+ [" dq{} ".format(n+1) for n in range(nRob)]
+ [" tau{} ".format(n+1) for n in range(nRob)])
# 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(", ")
f.write(fmtstr.format(*tau[k,:]))
f.write("\n")
As you can see in the left figure, the initial gain setting of Kp = 10 is not. Increase it to 50 or 100 and simulate again. You should see a similar behavior as in the middle and right figures. For these settings you already see an oscillating of the computed torque input. As there is still a deviation from the desired reference position, increase the controller gain to Kp = 1000. If you get an error message such as:
[IDA ERROR] IDASolve
At t = 0.208834, , mxstep steps taken before reaching tout.
most often means that the solver could not solve your problem, because the supplied input torques are too large or changing too rapidly.
Note
From the experiences I made I can say if this is an issue in the simulation it is also likely to be an issue in practice
One option to overcome this problem is to reduce the sampling time. Set it to Ts = 0.001. You should then get a result similar to the following:
However, you can also change your controller. If you have installed the python-lwrmodels package, you can compensate the gravitational effects for example. For this purpose, import the respective function to compute the gravitational torque:
from lwrmodels.lwr4_q2q4.dynamics import get_gravity_torque_vector as tau_g
Then add the computed gravity torque so that gravitational effects are compensated by the actuators:
# Add gravity compensation torque
cur_tau[ds.simulatedJoints] += tau_g(q[k-1,ds.simulatedJoints])
Note
The functions of lwrmodels return vectors and matrices of the modeled joints only. Therefore, we use the list of joints are described by the model, ds.simulatedJoints. It helps us to assign the modeled joints to the whole robot.
With the original settings of Kp = 10, Ts = 0.01, the simulated motion should be similar to the following figure: