#cython: boundscheck=False from libcpp.string cimport string from libcpp cimport bool from libcpp.vector cimport vector cdef extern from "" namespace "RigidBodyDynamics::Math": cdef cppclass VectorNd: VectorNd () VectorNd (int dim) int rows() int cols() void resize (int) double& operator[](int) double* data() cdef cppclass Vector3d: Vector3d () int rows() int cols() double& operator[](int) double* data() cdef cppclass Quaternion: Quaternion () int rows() int cols() double& operator[](int) double* data() Matrix3d toMatrix() # Quaternion fromMatrix (Matrix3d &mat) cdef cppclass SpatialVector: SpatialVector () int rows() int cols() double& operator[](int) double* data() cdef cppclass Matrix3d: Matrix3d () int rows() int cols() double& coeff "operator()"(int,int) double* data() cdef cppclass MatrixNd: MatrixNd () MatrixNd (int rows, int cols) int rows() int cols() void resize (int,int) double& coeff "operator()"(int,int) double* data() void setZero() cdef cppclass SpatialMatrix: SpatialMatrix () int rows() int cols() double& coeff "operator()"(int,int) double* data() cdef cppclass Matrix63: Matrix63 () int rows() int cols() double& coeff "operator()"(int,int) double* data() cdef extern from "" namespace "RigidBodyDynamics::Math::Quaternion": Quaternion fromMatrix(const Matrix3d &mat) cdef extern from "" namespace "RigidBodyDynamics::Math": cdef cppclass SpatialTransform: SpatialTransform() SpatialMatrix toMatrix() SpatialTransform inverse() SpatialTransform operator*(const SpatialTransform&) Matrix3d E Vector3d r cdef cppclass SpatialRigidBodyInertia: SpatialRigidBodyInertia() SpatialMatrix toMatrix() double m Vector3d h double Ixx, Iyx, Iyy, Izx, Izy, Izz cdef extern from "" namespace "RigidBodyDynamics": cdef cppclass Body: Body() Body(const double mass, const Vector3d &com, const Matrix3d &inertia) double mMass Vector3d mCenterOfMass Matrix3d mInertia bool mIsVirtual cdef cppclass FixedBody: FixedBody() double mMass Vector3d mCenterOfMass Matrix3d mInertia unsigned int mMovableParent SpatialTransform mParentTransform SpatialTransform mBaseTransform bool mIsVirtual cdef extern from "" namespace "RigidBodyDynamics": cdef enum JointType: JointTypeUndefined = 0 JointTypeRevolute JointTypePrismatic JointTypeRevoluteX JointTypeRevoluteY JointTypeRevoluteZ JointTypeSpherical JointTypeEulerZYX JointTypeEulerXYZ JointTypeEulerYXZ JointTypeTranslationXYZ JointTypeFloatingBase JointTypeFixed JointType1DoF JointType2DoF JointType3DoF JointType4DoF JointType5DoF JointType6DoF JointTypeCustom cdef extern from "" namespace "RigidBodyDynamics": cdef cppclass Joint: Joint() Joint(JointType joint_type) SpatialVector* mJointAxes JointType mJointType unsigned int mDoFCount unsigned int q_index cdef extern from "" namespace "RigidBodyDynamics": cdef cppclass Model: Model() unsigned int AddBody (const unsigned int parent_id, const SpatialTransform &joint_frame, const Joint &joint, const Body &body, string body_name ) unsigned int AppendBody (const SpatialTransform &joint_frame, const Joint &joint, const Body &body, string body_name ) unsigned int GetParentBodyId( unsigned int body_id) unsigned int GetBodyId( const char *body_name) string GetBodyName ( unsigned int body_id) bool IsBodyId ( unsigned int body_id) bool IsFixedBodyId ( unsigned int body_id) Quaternion GetQuaternion ( unsigned int body_id, const VectorNd &q) void SetQuaternion ( unsigned int body_id, const Quaternion &quat, VectorNd &q) vector[unsigned int] _lambda vector[unsigned int] lambda_q # vector[vector[unsigned int]] mu unsigned int dof_count unsigned int q_size unsigned int qdot_size unsigned int previously_added_body_id Vector3d gravity vector[SpatialVector] v vector[SpatialVector] a vector[Joint] mJoints vector[SpatialVector] S vector[SpatialVector] v_J vector[SpatialVector] c_J vector[unsigned int] mJointUpdateOrder vector[SpatialTransform] X_T vector[unsigned int] mFixedJointCount vector[Matrix63] multdof3_S vector[Matrix63] multdof3_U vector[Matrix63] multdof3_Dinv vector[Matrix63] multdof3_u vector[unsigned int] multdof3_w_index vector[SpatialVector] c vector[SpatialMatrix] IA vector[SpatialVector] pA vector[SpatialVector] U VectorNd d VectorNd u vector[SpatialVector] f vector[SpatialRigidBodyInertia] I vector[SpatialRigidBodyInertia] Ic vector[SpatialVector] hc vector[SpatialTransform] X_lambda vector[SpatialTransform] X_base vector[FixedBody] mFixedBodies unsigned int fixed_body_discriminator vector[Body] mBodies cdef extern from "" namespace "RigidBodyDynamics": cdef void UpdateKinematics (Model& model, const VectorNd &q, const VectorNd &qdot, const VectorNd &qddot) cdef Vector3d CalcBodyToBaseCoordinates (Model& model, const VectorNd &q, const unsigned int body_id, const Vector3d &body_point_coordinates, bool update_kinematics) cdef Vector3d CalcBaseToBodyCoordinates (Model& model, const VectorNd &q, const unsigned int body_id, const Vector3d &body_point_coordinates, bool update_kinematics) cdef Matrix3d CalcBodyWorldOrientation (Model& model, const VectorNd &q, const unsigned int body_id, bool update_kinematics) cdef Vector3d CalcPointVelocity (Model& model, const VectorNd &q, const VectorNd &qdot, const unsigned int body_id, const Vector3d &body_point_coordinates, bool update_kinematics) cdef Vector3d CalcPointAcceleration (Model& model, const VectorNd &q, const VectorNd &qdot, const VectorNd &qddot, const unsigned int body_id, const Vector3d &body_point_coordinates, bool update_kinematics) cdef SpatialVector CalcPointVelocity6D (Model& model, const VectorNd &q, const VectorNd &qdot, const unsigned int body_id, const Vector3d &body_point_coordinates, bool update_kinematics) cdef SpatialVector CalcPointAcceleration6D (Model& model, const VectorNd &q, const VectorNd &qdot, const VectorNd &qddot, const unsigned int body_id, const Vector3d &body_point_coordinates, bool update_kinematics) cdef Vector3d CalcAngularVelocityfromMatrix ( const Matrix3d &RotMat ) cdef cppclass InverseKinematicsConstraintSet: InverseKinematicsConstraintSet() unsigned int AddPointConstraint ( unsigned int body_id, const Vector3d &body_point, const Vector3d &target_pos ) unsigned int AddOrientationConstraint ( unsigned int body_id, const Matrix3d &target_orientation ) unsigned int AddFullConstraint ( unsigned int body_id, const Vector3d &body_point, const Vector3d &target_pos, const Matrix3d &target_orientation ) unsigned int ClearConstraints() unsigned int num_constraints double damper # lambda is built-in keyword unsigned int num_steps unsigned int max_steps double step_tol double constraint_tol double error_norm MatrixNd J # Jacobian of all constraints VectorNd e # Vector of all constraint residuals vector[unsigned int] body_ids vector[Vector3d] body_points vector[Vector3d] target_positions vector[Matrix3d] target_orientations cdef bool InverseKinematics ( Model &model, const VectorNd &Qinit, InverseKinematicsConstraintSet &CS, VectorNd &Qres ) cdef extern from "" namespace "RigidBodyDynamics::Utils": cdef void CalcCenterOfMass (Model& model, const VectorNd &q, const VectorNd &qdot, const VectorNd *qdot, double &mass, Vector3d &com, Vector3d *com_velocity, Vector3d *com_acceleration, Vector3d *angular_momentum, Vector3d *change_of_angular_momentum, bool update_kinematics) cdef void CalcZeroMomentPoint (Model& model, const VectorNd &q, const VectorNd &qdot, const VectorNd &qddot, Vector3d* zmp, const Vector3d &normal, const Vector3d &point, bool update_kinematics) cdef extern from "" namespace "RigidBodyDynamics": cdef cppclass ConstraintSet: ConstraintSet() unsigned int AddContactConstraint ( unsigned int body_id, const Vector3d &body_point, const Vector3d &world_normal, const char* constraint_name, double normal_acceleration) unsigned int AddLoopConstraint ( unsigned int id_predecessor, unsigned int id_successor, const SpatialTransform &X_predecessor, const SpatialTransform &X_successor, const SpatialVector &axis, bool enable_stabilization, double stabilization_param, const char *constraint_name) ConstraintSet Copy() # void SetSolver (Math::LinearSolver solver) bool Bind (const Model &model) size_t size() void clear() # Math::LinearSolver bool bound vector[string] name vector[unsigned int] body vector[Vector3d] point vector[Vector3d] normal VectorNd acceleration VectorNd force VectorNd impulse VectorNd v_plus MatrixNd H VectorNd C VectorNd gamma VectorNd G MatrixNd A VectorNd b VectorNd x MatrixNd GT_qr_Q MatrixNd Y MatrixNd Z VectorNd qddot_y VectorNd qddot_z MatrixNd K VectorNd a VectorNd QDDot_t VectorNd QDDot_0 vector[SpatialVector] f_t vector[SpatialVector] f_ext_constraints vector[Vector3d] point_accel_0 vector[SpatialVector] d_pA vector[SpatialVector] d_a VectorNd d_u vector[SpatialMatrix] d_IA vector[SpatialVector] d_U VectorNd d_d vector[Vector3d] d_multdof3_u cdef extern from "rbdl_ptr_functions.h" namespace "RigidBodyDynamics": cdef void CalcPointJacobianPtr (Model& model, const double *q_ptr, unsigned int body_id, const Vector3d &point_position, double *G, bool update_kinematics) cdef void CalcPointJacobian6DPtr (Model &model, const double *q_ptr, unsigned int body_id, const Vector3d &point_position, double *G, bool update_kinematics) cdef void CalcBodySpatialJacobianPtr ( Model &model, const double *q_ptr, unsigned int body_id, double *G, bool update_kinematics) cdef void InverseDynamicsPtr ( Model &model, const double* q_ptr, const double* qdot_ptr, const double* qddot_ptr, double* tau_ptr, vector[SpatialVector] *f_ext ) cdef void NonlinearEffectsPtr ( Model &model, const double* q_ptr, const double* qdot_ptr, double* tau_ptr ) cdef void CompositeRigidBodyAlgorithmPtr (Model& model, const double *q, double *H, bool update_kinematics) cdef void ForwardDynamicsPtr ( Model &model, const double* q_ptr, const double* qdot_ptr, double* tau_ptr, const double* qddot_ptr, vector[SpatialVector] *f_ext ) cdef void ForwardDynamicsConstraintsDirectPtr ( Model &model, const double* q_ptr, const double* qdot_ptr, const double* tau_ptr, ConstraintSet &CS, double* qddot_ptr ) cdef extern from "rbdl_loadmodel.cc": cdef bool rbdl_loadmodel ( const char* filename, Model* model, bool floating_base, bool verbose)