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))