def get_simulation( model, dt=0.1, tmax=5.0, b=0.5 ):
q = np.zeros (model.q_size)
qd = np.zeros (model.qdot_size)
qdd = np.zeros (model.qdot_size)
tau = np.zeros (model.qdot_size)
data = np.empty((0,1+model.qdot_size))
if q.shape == qd.shape:
for t in np.arange(0.0,tmax,dt):
d = np.hstack([t,(180.0/np.pi*q)])
data = np.vstack((data,d))
tau = -b*qd;
rbdl.ForwardDynamics (model, q, qd, tau, qdd)
q+=qd*dt
qd+=qdd*dt
else: # for quaternions
q[3]=1.0
for t in np.arange(0.0,tmax,dt):
d = np.hstack([t,(180.0/np.pi*QtoEulerZYX(q))])
data = np.vstack((data,d))
tau = -b*qd;
rbdl.ForwardDynamics (model, q, qd, tau, qdd)
Q = model.GetQuaternion(1,q)
norm2_w = np.dot(qd,qd)
if norm2_w>0.0001:
norm_w = np.sqrt(norm2_w)
Qw = QfromAxisAngle(qd/norm_w,dt*norm_w)
Q = Qmultiply(Qw,Q)
Q/=np.sqrt(np.dot(Q,Q))
model.SetQuaternion(1,Q,q)
qd+=qdd*dt
return data
def test_Dynamics_fextConsistency (self):
""" Checks whether forward and inverse dynamics are consistent """
q = np.random.rand (self.model.q_size)
qdot = np.random.rand (self.model.q_size)
qddot = np.random.rand (self.model.q_size)
qddot_fext = qddot
tau = np.random.rand (self.model.q_size)
forceA = np.zeros (6)
forceB = np.zeros (6)
forceC = np.zeros (6)
forceD = np.zeros (6)
fext = np.array([forceA, forceB, forceC, forceD])
rbdl.ForwardDynamics (
self.model,
q,
qdot,
tau,
qddot,
)
rbdl.ForwardDynamics (
self.model,
q,
qdot,
tau,
qddot_fext,
fext
)
tau_id = np.zeros ((self.model.q_size))
tau_id_fext = tau_id
rbdl.InverseDynamics (
self.model,
q,
qdot,
qddot,
tau_id,
)
rbdl.InverseDynamics (
self.model,
q,
qdot,
qddot,
tau_id_fext,
fext
)
assert_almost_equal (qddot, qddot_fext)
assert_almost_equal (tau_id, tau_id_fext)
def test_DynamicsConsistency (self):
""" Checks whether forward and inverse dynamics are consistent """
q = np.random.rand (self.model.q_size)
qdot = np.random.rand (self.model.q_size)
qddot = np.random.rand (self.model.q_size)
tau = np.random.rand (self.model.q_size)
rbdl.ForwardDynamics (
self.model,
q,
qdot,
tau,
qddot
)
tau_id = np.zeros ((self.model.q_size))
rbdl.InverseDynamics (
self.model,
q,
qdot,
qddot,
tau_id
)
assert_almost_equal (tau, tau_id)
def func(x, t, ftau, lua_model):
c = ftau(t)
nDof = lua_model.dof_count
#differetnial eq: dx/dt = [qd, qdd]
q_int = x[:nDof]
qd_int = x[nDof:]
qdd = np.zeros((2))
dxdt = np.zeros(x.shape)
rbdl.ForwardDynamics(lua_model, q_int, qd_int, c, qdd)
dxdt[:nDof] = qd_int[:]
dxdt[nDof:] = qdd[:]
return dxdt
def forward_dynamics(self, tau, q=None, qdot=None):
"""
Calculate the joint accelerations from given joint torques, joint
positions and joint velocities.
:param tau: Joint torques.
:param q: Joint positions. It uses the current model positions if it is
not specified.
:param qdot: Joint accelerations. It uses the current model
accelerations if it is not specified.
:return: Joint Accelerations
"""
if q is None:
q = self.q
if qdot is None:
qdot = self.qdot
qddot = np.zeros(self.qdot_size)
rbdl.ForwardDynamics(self.model, q, qdot, tau, qddot)
return qddot
def rhs(model, y, tau):
dim = model.dof_count
res = np.zeros(dim * 2)
Q = np.zeros(model.q_size)
QDot = np.zeros(model.qdot_size)
QDDot = np.zeros(model.qdot_size)
Tau = tau
for i in range(0, dim):
Q[i] = y[i]
QDot[i] = y[i + dim]
rbdl.ForwardDynamics(model, Q, QDot, Tau, QDDot)
for i in range(0, dim):
res[i] = QDot[i]
res[i + dim] = QDDot[i]
return res
def rhs(model, y, tau):
# get the nessary values and init some vars
dim = model.dof_count
res = np.zeros(dim * 2)
Q = np.zeros(model.q_size)
QDot = np.zeros(model.qdot_size)
QDDot = np.zeros(model.qdot_size)
Tau = tau
for i in range(0, dim):
Q[i] = y[i]
QDot[i] = y[i + dim]
# forward simulate the model using RBDL
rbdl.ForwardDynamics(model, Q, QDot, Tau, QDDot)
# update the step
for i in range(0, dim):
res[i] = QDot[i]
res[i + dim] = QDDot[i]
return res
def func(x, t,ftau,lua_model):
#integrator
#differetnial eq: dx/dt = [qd, qdd]
q_int = np.zeros((2))
qd_int = np.zeros((2))
qdd = np.zeros((2))
dxdt = np.zeros(xState_0.shape)
c=ftau(t)
for i in range(nDof):
q_int[i] = x[i]
qd_int[i] = x[i+nDof]
rbdl.ForwardDynamics(lua_model,q_int,qd_int,c,qdd)
k=0
for j in range(nDof):
dxdt[k] = qd_int[j]
k=k+1
for j in range(nDof):
dxdt[k] = qdd[j]
k=k+1
return dxdt
def calculate(self, model, q, qdot, u):
qddot0 = np.empty_like(q)
qddot1 = np.empty_like(q)
dq = 1e-4
du = 1e-4
ss = LinearSystem(len(q), len(u))
ss.A[:len(q), len(q):] = np.eye(len(q))
for i in range(len(q)):
q1 = np.copy(q)
q1[i] = q[i] - dq / 2
rbdl.ForwardDynamics(model, q1, qdot, u, qddot0)
q1[i] = q[i] + dq / 2
rbdl.ForwardDynamics(model, q1, qdot, u, qddot1)
ss.A[len(q):, i] = (qddot1 - qddot0) / dq
qdot1 = np.copy(qdot)
qdot1[i] = qdot[i] - dq / 2
rbdl.ForwardDynamics(model, q, qdot1, u, qddot0)
qdot1[i] = qdot[i] + dq / 2
rbdl.ForwardDynamics(model, q, qdot1, u, qddot1)
ss.A[len(q):, i + len(q)] = (qddot1 - qddot0) / dq
for i in range(len(u)):
u1 = np.copy(u)
u1[i] = u[i] - du / 2
rbdl.ForwardDynamics(model, q, qdot, u1, qddot0)
u1[i] = u[i] + du / 2
rbdl.ForwardDynamics(model, q, qdot, u1, qddot1)
ss.B[len(q):, i] = (qddot1 - qddot0) / du
ss.C = qddot0
return ss
fd_sym = kuka.get_forward_dynamics_crba(root, tip, gravity)
error = np.zeros(n_joints)
def u2c2np(asd):
return cs.Function("temp",[],[asd])()["o0"].toarray()
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
tau[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2
q_rbdl[j] = q[j]
qdot_rbdl[j] = qdot[j]
tau_rbdl[j] = tau[j]
rbdl.ForwardDynamics(kuka_model, q_rbdl, qdot_rbdl, tau_rbdl, fd_rbdl)
fd_u2c = fd_sym(q, qdot, tau)
for fd_idx in range(n_joints):
error[fd_idx] += np.absolute(fd_rbdl[fd_idx] - u2c2np(fd_u2c)[fd_idx])
print "Errors in forward dynamics joint accelerations with",n_itr, "iterations and comparing against rbdl:\n", error
sum_error = 0
for err in range(6):
sum_error += error[err]
print "Sum of errors:\n", sum_error
return cs.Function("temp", [], [asd])()["o0"].toarray()
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
tau[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
fd_u2c_crba = fd_sym_crba(q, qdot, tau)
fd_u2c_aba = fd_sym_aba(q, qdot, tau)
rbdl.ForwardDynamics(ur5_rbdl, q, qdot, tau, fd_rbdl_aba)
#rbdl.ForwardDynamicsLagrangian(ur5_rbdl, q, qdot, tau, fd_rbdl_crba)
for qddot_idx in range(n_joints):
error_rbdl_u2c_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_aba[qddot_idx]) - fd_rbdl_aba[qddot_idx])
#error_rbdl_u2c_crba[qddot_idx] += np.absolute(u2c2np(fd_u2c_crba[qddot_idx]) - fd_rbdl_crba[qddot_idx])
error_u2c_crba_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_aba[qddot_idx]) - u2c2np(fd_u2c_crba[qddot_idx]))
#error_rbdl_crba_aba[qddot_idx] += np.absolute(fd_rbdl_crba[qddot_idx] - fd_rbdl_aba[qddot_idx])
error_u2c_crba_rbdl_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_crba[qddot_idx]) - fd_rbdl_aba[qddot_idx])
#sum_error_rbdl_u2c_crba = 0
sum_error_rbdl_u2c_aba = 0
def integrate(self, q, qdot, tau, dt):
rbdl.ForwardDynamics(self.model, q, qdot, tau, self.qddot)
q += qdot * dt
qdot += self.qddot * dt
return cs.Function("temp", [], [asd])()["o0"].toarray()
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
tau[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
fd_u2c_crba = fd_sym_crba(q, qdot, tau)
fd_u2c_aba = fd_sym_aba(q, qdot, tau)
rbdl.ForwardDynamics(snake_rbdl, q, qdot, tau, fd_rbdl_aba)
#rbdl.ForwardDynamicsLagrangian(snake_rbdl, q, qdot, tau, fd_rbdl_crba)
for qddot_idx in range(n_joints):
error_rbdl_u2c_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_aba[qddot_idx]) - fd_rbdl_aba[qddot_idx])
#error_rbdl_u2c_crba[qddot_idx] += np.absolute(u2c2np(fd_u2c_crba[qddot_idx]) - fd_rbdl_crba[qddot_idx])
error_u2c_crba_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_aba[qddot_idx]) - u2c2np(fd_u2c_crba[qddot_idx]))
#error_rbdl_crba_aba[qddot_idx] += np.absolute(fd_rbdl_crba[qddot_idx] - fd_rbdl_aba[qddot_idx])
error_u2c_crba_rbdl_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_crba[qddot_idx]) - fd_rbdl_aba[qddot_idx])
sum_error_rbdl_u2c_crba = 0
sum_error_rbdl_u2c_aba = 0
return cs.Function("temp", [], [asd])()["o0"].toarray()
n_itr = 1000
for i in range(n_itr):
for j in range(n_joints):
q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
tau[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] -
q_min[j]) / 2
fd_u2c_crba = fd_sym_crba(q, qdot, tau)
fd_u2c_aba = fd_sym_aba(q, qdot, tau)
rbdl.ForwardDynamics(gantry_rbdl, q, qdot, tau, fd_rbdl_aba)
#rbdl.ForwardDynamicsLagrangian(gantry_rbdl, q, qdot, tau, fd_rbdl_crba)
for qddot_idx in range(n_joints):
error_rbdl_u2c_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_aba[qddot_idx]) - fd_rbdl_aba[qddot_idx])
#error_rbdl_u2c_crba[qddot_idx] += np.absolute(u2c2np(fd_u2c_crba[qddot_idx]) - fd_rbdl_crba[qddot_idx])
error_u2c_crba_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_aba[qddot_idx]) - u2c2np(fd_u2c_crba[qddot_idx]))
#error_rbdl_crba_aba[qddot_idx] += np.absolute(fd_rbdl_crba[qddot_idx] - fd_rbdl_aba[qddot_idx])
error_u2c_crba_rbdl_aba[qddot_idx] += np.absolute(
u2c2np(fd_u2c_crba[qddot_idx]) - fd_rbdl_aba[qddot_idx])
sum_error_rbdl_u2c_crba = 0
sum_error_rbdl_u2c_aba = 0
def calculate_acceleration(self):
rbdl.ForwardDynamics(self.model, self.q, self.qd, self.torque,
self.qdd_tocheck)
# Construct the model
body_1 = model.AppendBody (rbdl.SpatialTransform(), joint_rot_y, body)
body_2 = model.AppendBody (xtrans, joint_rot_y, body)
body_3 = model.AppendBody (xtrans, joint_rot_y, body)
# Create numpy arrays for the state
q = np.zeros (model.q_size)
qdot = np.zeros (model.qdot_size)
qddot = np.zeros (model.qdot_size)
tau = np.zeros (model.qdot_size)
# Modify the state
q[0] = 1.3
q[1] = -0.5
q[2] = 3.2
# Transform coordinates from local to global coordinates
point_local = np.array([1., 2., 3.])
point_base = rbdl.CalcBodyToBaseCoordinates (model, q, body_3, point_local)
point_local_2 = rbdl.CalcBaseToBodyCoordinates (model, q, body_3, point_base)
# Perform forward dynamics and print the result
rbdl.ForwardDynamics (model, q, qdot, tau, qddot)
print ("qddot = " + str(qddot.transpose()))
# Compute and print the jacobian (note: the output parameter
# of the Jacobian G must have proper size!)
G = np.zeros([3,model.qdot_size])
rbdl.CalcPointJacobian (model, q, body_3, point_local, G)
print ("G = \n" + str(G))
