Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • sw/simox/simox
  • uwkce_singer/simox
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 3313 deletions
3rdParty/rbdl/include/rbdl/rbdl_math.h deleted 100644 → 0
View file @ a5f65699
/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#ifndef RBDL_MATH_H
#define RBDL_MATH_H
#include <rbdl/rbdl_config.h>
#ifdef RBDL_USE_SIMPLE_MATH
#include <rbdl/SimpleMath/SimpleMathFixed.h>
#include <rbdl/SimpleMath/SimpleMathDynamic.h>
#include <rbdl/SimpleMath/SimpleMathMixed.h>
#include <rbdl/SimpleMath/SimpleMathQR.h>
#include <rbdl/SimpleMath/SimpleMathCholesky.h>
#include <rbdl/SimpleMath/SimpleMathCommaInitializer.h>
#include <rbdl/SimpleMath/SimpleMathMap.h>
#include <vector>
typedef SimpleMath::Fixed::Matrix<double, 2,1> Vector2_t;
typedef SimpleMath::Fixed::Matrix<double, 3,1> Vector3_t;
typedef SimpleMath::Fixed::Matrix<double, 3,3> Matrix3_t;
typedef SimpleMath::Fixed::Matrix<double, 4,1> Vector4_t;
typedef SimpleMath::Fixed::Matrix<double, 6,1> SpatialVector_t;
typedef SimpleMath::Fixed::Matrix<double, 6,6> SpatialMatrix_t;
typedef SimpleMath::Fixed::Matrix<double, 6,3> Matrix63_t;
typedef SimpleMath::Fixed::Matrix<double, 4,3> Matrix43_t;
typedef SimpleMath::Dynamic::Matrix<double> MatrixN_t;
typedef SimpleMath::Dynamic::Matrix<double> VectorN_t;
#else
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <Eigen/QR>
#include <rbdl/rbdl_eigenmath.h>
typedef Eigen::Matrix<double, 6, 3> Matrix63_t;
typedef Eigen::Matrix<double, 4, 3> Matrix43_t;
typedef Eigen::VectorXd VectorN_t;
typedef Eigen::MatrixXd MatrixN_t;
#endif
namespace RigidBodyDynamics {
/** \brief Math types such as vectors and matrices and utility functions. */
namespace Math {
typedef Vector2_t Vector2d;
typedef Vector3_t Vector3d;
typedef Vector4_t Vector4d;
typedef Matrix3_t Matrix3d;
typedef SpatialVector_t SpatialVector;
typedef SpatialMatrix_t SpatialMatrix;
typedef Matrix63_t Matrix63;
typedef Matrix43_t Matrix43;
typedef VectorN_t VectorNd;
typedef MatrixN_t MatrixNd;
} /* Math */
} /* RigidBodyDynamics */
#include <rbdl/Quaternion.h>
#include <rbdl/SpatialAlgebraOperators.h>
// If we use Eigen3 we have to create specializations of the STL
// std::vector such that the alignment is done properly.
#ifndef RBDL_USE_SIMPLE_MATH
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialVector)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialMatrix)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix63)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::Matrix43)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialTransform)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Math::SpatialRigidBodyInertia)
#endif
/* RBDL_MATH_H_H */
#endif
3rdParty/rbdl/include/rbdl/rbdl_mathutils.h deleted 100644 → 0
View file @ a5f65699
/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#ifndef RBDL_MATHUTILS_H
#define RBDL_MATHUTILS_H
#include <assert.h>
#include <cmath>
#include <rbdl/rbdl_math.h>
namespace RigidBodyDynamics {
struct Model;
namespace Math {
/** \brief Available solver methods for the linear systems.
*
* Please note that these methods are only available when Eigen3 is used.
* When the math library SimpleMath is used it will always use a slow
* column pivoting gauss elimination.
*/
enum RBDL_DLLAPI LinearSolver {
LinearSolverUnknown = 0,
LinearSolverPartialPivLU,
LinearSolverColPivHouseholderQR,
LinearSolverHouseholderQR,
LinearSolverLLT,
LinearSolverLast,
};
extern RBDL_DLLAPI Vector3d Vector3dZero;
extern RBDL_DLLAPI Matrix3d Matrix3dIdentity;
extern RBDL_DLLAPI Matrix3d Matrix3dZero;
RBDL_DLLAPI inline VectorNd VectorFromPtr (unsigned int n, double *ptr) {
// TODO: use memory mapping operators for Eigen
VectorNd result (n);
for (unsigned int i = 0; i < n; i++) {
result[i] = ptr[i];
}
return result;
}
RBDL_DLLAPI inline MatrixNd MatrixFromPtr (unsigned int rows, unsigned int cols, double *ptr, bool row_major = true) {
MatrixNd result (rows, cols);
if (row_major) {
for (unsigned int i = 0; i < rows; i++) {
for (unsigned int j = 0; j < cols; j++) {
result(i,j) = ptr[i * cols + j];
}
}
} else {
for (unsigned int i = 0; i < rows; i++) {
for (unsigned int j = 0; j < cols; j++) {
result(i,j) = ptr[i + j * rows];
}
}
}
return result;
}
/// \brief Solves a linear system using gaussian elimination with pivoting
RBDL_DLLAPI bool LinSolveGaussElimPivot (MatrixNd A, VectorNd b, VectorNd &x);
// \todo write test
RBDL_DLLAPI void SpatialMatrixSetSubmatrix(SpatialMatrix &dest, unsigned int row, unsigned int col, const Matrix3d &matrix);
RBDL_DLLAPI bool SpatialMatrixCompareEpsilon (const SpatialMatrix &matrix_a,
const SpatialMatrix &matrix_b, double epsilon);
RBDL_DLLAPI bool SpatialVectorCompareEpsilon (const SpatialVector &vector_a,
const SpatialVector &vector_b, double epsilon);
/** \brief Translates the inertia matrix to a new center. */
RBDL_DLLAPI Matrix3d parallel_axis (const Matrix3d &inertia, double mass, const Vector3d &com);
/** \brief Creates a transformation of a linear displacement
*
* This can be used to specify the translation to the joint center when
* adding a body to a model. See also section 2.8 in RBDA.
*
* \note The transformation returned is for motions. For a transformation for forces
* \note one has to conjugate the matrix.
*
* \param displacement The displacement as a 3D vector
*/
RBDL_DLLAPI SpatialMatrix Xtrans_mat (const Vector3d &displacement);
/** \brief Creates a rotational transformation around the Z-axis
*
* Creates a rotation around the current Z-axis by the given angle
* (specified in radians).
*
* \param zrot Rotation angle in radians.
*/
RBDL_DLLAPI SpatialMatrix Xrotz_mat (const double &zrot);
/** \brief Creates a rotational transformation around the Y-axis
*
* Creates a rotation around the current Y-axis by the given angle
* (specified in radians).
*
* \param yrot Rotation angle in radians.
*/
RBDL_DLLAPI SpatialMatrix Xroty_mat (const double &yrot);
/** \brief Creates a rotational transformation around the X-axis
*
* Creates a rotation around the current X-axis by the given angle
* (specified in radians).
*
* \param xrot Rotation angle in radians.
*/
RBDL_DLLAPI SpatialMatrix Xrotx_mat (const double &xrot);
/** \brief Creates a spatial transformation for given parameters
*
* Creates a transformation to a coordinate system that is first rotated
* and then translated.
*
* \param displacement The displacement to the new origin
* \param zyx_euler The orientation of the new coordinate system, specifyed
* by ZYX-Euler angles.
*/
RBDL_DLLAPI SpatialMatrix XtransRotZYXEuler (const Vector3d &displacement, const Vector3d &zyx_euler);
RBDL_DLLAPI inline Matrix3d rotx (const double &xrot) {
double s, c;
s = sin (xrot);
c = cos (xrot);
return Matrix3d (
1., 0., 0.,
0., c, s,
0., -s, c
);
}
RBDL_DLLAPI inline Matrix3d roty (const double &yrot) {
double s, c;
s = sin (yrot);
c = cos (yrot);
return Matrix3d (
c, 0., -s,
0., 1., 0.,
s, 0., c
);
}
RBDL_DLLAPI inline Matrix3d rotz (const double &zrot) {
double s, c;
s = sin (zrot);
c = cos (zrot);
return Matrix3d (
c, s, 0.,
-s, c, 0.,
0., 0., 1.
);
}
RBDL_DLLAPI inline Matrix3d rotxdot (const double &x, const double &xdot) {
double s, c;
s = sin (x);
c = cos (x);
return Matrix3d (
0., 0., 0.,
0., -s * xdot, c * xdot,
0., -c * xdot,-s * xdot
);
}
RBDL_DLLAPI inline Matrix3d rotydot (const double &y, const double &ydot) {
double s, c;
s = sin (y);
c = cos (y);
return Matrix3d (
-s * ydot, 0., - c * ydot,
0., 0., 0.,
c * ydot, 0., - s * ydot
);
}
RBDL_DLLAPI inline Matrix3d rotzdot (const double &z, const double &zdot) {
double s, c;
s = sin (z);
c = cos (z);
return Matrix3d (
-s * zdot, c * zdot, 0.,
-c * zdot, -s * zdot, 0.,
0., 0., 0.
);
}
RBDL_DLLAPI inline Vector3d angular_velocity_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates) {
double sy = sin(zyx_angles[1]);
double cy = cos(zyx_angles[1]);
double sx = sin(zyx_angles[2]);
double cx = cos(zyx_angles[2]);
return Vector3d (
zyx_angle_rates[2] - sy * zyx_angle_rates[0],
cx * zyx_angle_rates[1] + sx * cy * zyx_angle_rates[0],
-sx * zyx_angle_rates[1] + cx * cy * zyx_angle_rates[0]
);
}
RBDL_DLLAPI inline Vector3d global_angular_velocity_from_rates (const Vector3d &zyx_angles, const Vector3d &zyx_rates) {
Matrix3d RzT = rotz(zyx_angles[0]).transpose();
Matrix3d RyT = roty(zyx_angles[1]).transpose();
return Vector3d (
Vector3d (0., 0., zyx_rates[0])
+ RzT * Vector3d (0., zyx_rates[1], 0.)
+ RzT * RyT * Vector3d (zyx_rates[2], 0., 0.)
);
}
RBDL_DLLAPI inline Vector3d angular_acceleration_from_angle_rates (const Vector3d &zyx_angles, const Vector3d &zyx_angle_rates, const Vector3d &zyx_angle_rates_dot) {
double sy = sin(zyx_angles[1]);
double cy = cos(zyx_angles[1]);
double sx = sin(zyx_angles[2]);
double cx = cos(zyx_angles[2]);
double xdot = zyx_angle_rates[2];
double ydot = zyx_angle_rates[1];
double zdot = zyx_angle_rates[0];
double xddot = zyx_angle_rates_dot[2];
double yddot = zyx_angle_rates_dot[1];
double zddot = zyx_angle_rates_dot[0];
return Vector3d (
xddot - (cy * ydot * zdot + sy * zddot),
-sx * xdot * ydot + cx * yddot + cx * xdot * cy * zdot + sx * ( - sy * ydot * zdot + cy * zddot),
-cx * xdot * ydot - sx * yddot - sx * xdot * cy * zdot + cx * ( - sy * ydot * zdot + cy * zddot)
);
}
RBDL_DLLAPI
void SparseFactorizeLTL (Model &model, Math::MatrixNd &H);
RBDL_DLLAPI
void SparseMultiplyHx (Model &model, Math::MatrixNd &L);
RBDL_DLLAPI
void SparseMultiplyLx (Model &model, Math::MatrixNd &L);
RBDL_DLLAPI
void SparseMultiplyLTx (Model &model, Math::MatrixNd &L);
RBDL_DLLAPI
void SparseSolveLx (Model &model, Math::MatrixNd &L, Math::VectorNd &x);
RBDL_DLLAPI
void SparseSolveLTx (Model &model, Math::MatrixNd &L, Math::VectorNd &x);
} /* Math */
} /* RigidBodyDynamics */
/* RBDL_MATHUTILS_H */
#endif
3rdParty/rbdl/include/rbdl/rbdl_utils.h deleted 100644 → 0
View file @ a5f65699
/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#ifndef RBDL_UTILS_H
#define RBDL_UTILS_H
#include <string>
#include <rbdl/rbdl_config.h>
#include <rbdl/rbdl_math.h>
namespace RigidBodyDynamics {
struct Model;
/** \brief Namespace that contains optional helper functions */
namespace Utils {
/** \brief Creates a human readable overview of the model. */
RBDL_DLLAPI std::string GetModelHierarchy (const Model &model);
/** \brief Creates a human readable overview of the Degrees of Freedom. */
RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model);
/** \brief Creates a human readable overview of the locations of all bodies that have names. */
RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model);
/** \brief Computes the Center of Mass (COM) and optionally its linear velocity.
*
* When only interested in computing the location of the COM you can use
* NULL as value for com_velocity.
*
* \param model The model for which we want to compute the COM
* \param q The current joint positions
* \param qdot The current joint velocities
* \param mass (output) total mass of the model
* \param com (output) location of the Center of Mass of the model in base coordinates
* \param qddot (optional input) A pointer to the current joint accelerations
* \param com_velocity (optional output) linear velocity of the COM in base coordinates
* \param com_acceleration (optional output) linear acceleration of the COM in base coordinates
* \param angular_momentum (optional output) angular momentum of the model at the COM in base coordinates
* \param change_of_angular_momentum (optional output) change of angular momentum of the model at the COM in base coordinates
* \param update_kinematics (optional input) whether the kinematics should be updated (defaults to true)
*
* \note When wanting to compute com_acceleration or change_of_angular
* momentum one has to provide qddot. In all other cases one can use NULL
* as argument for qddot.
*/
RBDL_DLLAPI void CalcCenterOfMass (
Model &model,
const Math::VectorNd &q,
const Math::VectorNd &qdot,
const Math::VectorNd *qddot,
double &mass,
Math::Vector3d &com,
Math::Vector3d *com_velocity = NULL,
Math::Vector3d *com_acceleration = NULL,
Math::Vector3d *angular_momentum = NULL,
Math::Vector3d *change_of_angular_momentum = NULL,
bool update_kinematics = true
);
/** \brief Computes the Zero-Moment-Point (ZMP) on a given contact surface.
*
* \param model The model for which we want to compute the ZMP
* \param q The current joint positions
* \param qdot The current joint velocities
* \param qdot The current joint accelerations
* \param normal The normal of the respective contact surface
* \param point A point on the contact surface
* \param zmp (output) location of the Zero-Moment-Point of the model in base coordinates projected on the given contact surface
* \param update_kinematics (optional input) whether the kinematics should be updated (defaults to true)
*/
RBDL_DLLAPI void CalcZeroMomentPoint (
Model &model,
const Math::VectorNd &q, const Math::VectorNd &qdot, const Math::VectorNd &qddot,
Math::Vector3d* zmp,
const Math::Vector3d &normal = Math::Vector3d (0., 0., 1.),
const Math::Vector3d &point = Math::Vector3d (0., 0., 0.),
bool update_kinematics = true
);
/** \brief Computes the potential energy of the full model. */
RBDL_DLLAPI double CalcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics = true);
/** \brief Computes the kinetic energy of the full model. */
RBDL_DLLAPI double CalcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics = true);
}
}
/* RBDL_UTILS_H */
#endif
3rdParty/rbdl/python/CMakeLists.txt deleted 100644 → 0
View file @ a5f65699
FIND_PROGRAM ( PYTHON "python" )
SET (Python_ADDITIONAL_VERSIONS 2.7)
INCLUDE ( UseCython )
CONFIGURE_FILE (
${CMAKE_CURRENT_SOURCE_DIR}/setup.py.cmake
${CMAKE_CURRENT_BINARY_DIR}/setup.py
)
# Process the rbdl-wrapper.pyx to generate rbdl.pyx BEFORE build time
# else cython_add_module will complain on configuration
EXECUTE_PROCESS (
COMMAND ${PYTHON} ${CMAKE_CURRENT_SOURCE_DIR}/wrappergen.py ${CMAKE_CURRENT_SOURCE_DIR}/rbdl-wrapper.pyx ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx
OUTPUT_QUIET
)
# Enable C++11 (or C++0x for older compilers)
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
# If the pyx file is a C++ file, we should specify that here.
set_source_files_properties( ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx
PROPERTIES CYTHON_IS_CXX TRUE )
# Multi-file cython modules do not appear to be working at the moment.
# NOTE there is some directory property problem in UseCython when build
# from CMAKE_CURRENT_BINARY_DIR
cython_add_module( rbdl-python ${CMAKE_CURRENT_SOURCE_DIR}/rbdl.pyx )
#SET_TARGET_PROPERTIES ( rbdl-python PROPERTIES PREFIX "")
SET_TARGET_PROPERTIES ( rbdl-python PROPERTIES OUTPUT_NAME "rbdl")
INCLUDE_DIRECTORIES (
${PROJECT_SOURCE_DIR}/include
${PROJECT_SOURCE_DIR}/python
${PROJECT_SOURCE_DIR}
)
TARGET_LINK_LIBRARIES (rbdl-python rbdl )
IF (RBDL_BUILD_ADDON_LUAMODEL)
TARGET_LINK_LIBRARIES (rbdl-python rbdl_luamodel )
ENDIF()
IF (RBDL_BUILD_ADDON_URDFREADER)
TARGET_LINK_LIBRARIES (rbdl-python rbdl_urdfreader )
ENDIF()
INSTALL ( TARGETS rbdl-python
LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/python2.7/site-packages/
)
3rdParty/rbdl/python/README.md deleted 100644 → 0
View file @ a5f65699
# Python wrapper for RBDL
This wrapper uses Cython to wrap RBDL in a Python module. The code requires
C++11 features and must be compiled with the flags ```-std=c++11``` (or on
older compilers: ```-std=c++0x```). It closely follows the C++ API. All
functions are found in the module ```rbdl```, e.g.
rbdl.ForwardDynamics (model, q, qdot, tau, qddot)
computes the accelerations qddot of the forward dynamics for given model,
q, qdot, tau.
Arguments are all passed as reference and where possible, the wrapper
avoids copying values, especially for larger matrices such as Jacobians,
and joint-space inertia matrix.
All functions have embedded signatures to ease the use from within IPython,
e.g. ```rbdl.ForwardDynamics?``` shows required arguments for the function.
# Highlights
Wrappers for the following features are already implemented:
* supports model creation from Python code
* supports model loading from LuaModel and URDF
* operates directly on raw numpy data for Jacobians and joint-space inertia
matrix - no copying required!
* direct access to almost all values of the Model structure
* all functions of Dynamics.h are wrapped:
- O(n) inverse dynamics via RNEA
- O(n) forward dynamics via ABA
- Coriolis term computation via simplified RNEA
- computation of joint-space inertia matrix via CRBA
* kinematic computations:
- body <-> world transformations
- body point positions, velocities, and accelerations, including their
6-D counterparts
- point Jacobians (translations)
- 6-D Jacobians (angular and linear movement)
- Spatial 6-D body Jacobians
* model mass, Center of Mass (CoM), CoM velocity, centroidal angular momentum
# Differences to the C++ API
The wrapper function ```rbdl.CalcCenterOfMass``` has a scalar return value
which is the mass of the model. Therefore the function does not use the
mass parameter when calling it.
# ToDo
* wrapping of constraint sets, and contact dynamics
* inverse kinematics
* documentation
3rdParty/rbdl/python/crbdl.pxd deleted 100644 → 0
View file @ a5f65699
#cython: boundscheck=False
from libcpp.string cimport string
from libcpp cimport bool
from libcpp.vector cimport vector
cdef extern from "<rbdl/rbdl_math.h>" 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 "<rbdl/Quaternion.h>" namespace "RigidBodyDynamics::Math::Quaternion":
Quaternion fromMatrix(const Matrix3d &mat)
cdef extern from "<rbdl/SpatialAlgebraOperators.h>" 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 "<rbdl/Body.h>" 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 "<rbdl/Joint.h>" 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 "<rbdl/Joint.h>" namespace "RigidBodyDynamics":
cdef cppclass Joint:
Joint()
Joint(JointType joint_type)
SpatialVector* mJointAxes
JointType mJointType
unsigned int mDoFCount
unsigned int q_index
cdef extern from "<rbdl/Model.h>" 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[SpatialTransform] X_J
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 "<rbdl/Kinematics.h>" 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 extern from "<rbdl/rbdl_utils.h>" 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 "<rbdl/Constraints.h>" 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)
3rdParty/rbdl/python/rbdl-wrapper.pyx deleted 100644 → 0
View file @ a5f65699
#cython: boundscheck=False, embedsignature=True
import numpy as np
cimport numpy as np
from libc.stdint cimport uintptr_t
from libcpp.string cimport string
cimport crbdl
##############################
#
# Linear Algebra Types
#
##############################
cdef class Vector3d:
cdef crbdl.Vector3d *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0, pyvalues=None):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.Vector3d()
if pyvalues is not None:
for i in range (3):
self.thisptr.data()[i] = pyvalues[i]
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.Vector3d*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "Vector3d [{:3.4f}, {:3.4f}, {:3.4f}]".format (
self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2])
def __getitem__(self, key):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))]
else:
return self.thisptr.data()[key]
def __setitem__(self, key, value):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
src_index = 0
for i in xrange (*key.indices(len(self))):
self.thisptr.data()[i] = value[src_index]
src_index = src_index + 1
else:
self.thisptr.data()[key] = value
def __len__ (self):
return 3
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return Vector3d (address)
@classmethod
def fromPythonArray (cls, python_values):
return Vector3d (0, python_values)
cdef class Matrix3d:
cdef crbdl.Matrix3d *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0, pyvalues=None):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.Matrix3d()
if pyvalues is not None:
for i in range (3):
for j in range (3):
(&(self.thisptr.coeff(i,j)))[0] = pyvalues[i,j]
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.Matrix3d*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "Matrix3d [{:3.4f}, {:3.4f}, {:3.4f}]".format (
self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2])
def __getitem__(self, key):
return self.thisptr.data()[key]
def __setitem__(self, key, value):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
src_index = 0
for i in xrange (*key.indices(len(self))):
self.thisptr.data()[i] = value[src_index]
src_index = src_index + 1
else:
self.thisptr.data()[key] = value
def __len__ (self):
return 3
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return Matrix3d (address)
@classmethod
def fromPythonArray (cls, python_values):
return Matrix3d (0, python_values)
cdef class VectorNd:
cdef crbdl.VectorNd *thisptr
cdef free_on_dealloc
def __cinit__(self, ndim, uintptr_t address=0, pyvalues=None):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.VectorNd(ndim)
if pyvalues is not None:
for i in range (ndim):
self.thisptr.data()[i] = pyvalues[i]
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.VectorNd*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __getitem__(self, key):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))]
else:
return self.thisptr.data()[key]
def __setitem__(self, key, value):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
src_index = 0
for i in xrange (*key.indices(len(self))):
self.thisptr.data()[i] = value[src_index]
src_index = src_index + 1
else:
self.thisptr.data()[key] = value
def __len__ (self):
return self.thisptr.rows()
def toNumpy (self):
result = np.ndarray (self.thisptr.rows())
for i in range (0, self.thisptr.rows()):
result[i] = self.thisptr[0][i]
return result
# Constructors
@classmethod
def fromPythonArray (cls, python_values):
return VectorNd (len(python_values), 0, python_values)
@classmethod
def fromPointer(cls, uintptr_t address):
cdef crbdl.VectorNd* vector_ptr = <crbdl.VectorNd*> address
return VectorNd (vector_ptr.rows(), <uintptr_t> address)
cdef class Quaternion:
cdef crbdl.Quaternion *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0, pyvalues=None, pymatvalues=None):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.Quaternion()
if pyvalues is not None:
for i in range (4):
self.thisptr.data()[i] = pyvalues[i]
elif pymatvalues is not None:
mat = Matrix3d()
for i in range (3):
for j in range (3):
(&(mat.thisptr.coeff(i,j)))[0] = pymatvalues[i,j]
self.thisptr[0] = crbdl.fromMatrix (mat.thisptr[0])
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.Quaternion*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "Quaternion [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4}]".format (
self.thisptr.data()[0], self.thisptr.data()[1],
self.thisptr.data()[2], self.thisptr.data()[3])
def __getitem__(self, key):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))]
else:
return self.thisptr.data()[key]
def __setitem__(self, key, value):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
src_index = 0
for i in xrange (*key.indices(len(self))):
self.thisptr.data()[i] = value[src_index]
src_index = src_index + 1
else:
self.thisptr.data()[key] = value
def __len__ (self):
return 4
def toMatrix(self):
cdef crbdl.Matrix3d mat
mat = self.thisptr.toMatrix()
result = np.array ([3,3])
for i in range (3):
for j in range (3):
result[i,j] = mat.coeff(i,j)
return result
def toNumpy(self):
result = np.ndarray (self.thisptr.rows())
for i in range (0, self.thisptr.rows()):
result[i] = self.thisptr[0][i]
return result
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return Quaternion (address)
@classmethod
def fromPythonArray (cls, python_values):
return Quaternion (0, python_values)
@classmethod
def fromPythonMatrix (cls, python_matrix_values):
return Quaternion (0, None, python_matrix_values)
cdef class SpatialVector:
cdef crbdl.SpatialVector *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0, pyvalues=None):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.SpatialVector()
if pyvalues is not None:
for i in range (6):
self.thisptr.data()[i] = pyvalues[i]
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.SpatialVector*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "SpatialVector [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format (
self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2],
self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5])
def __getitem__(self, key):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
return [self.thisptr.data()[i] for i in xrange(*key.indices(len(self)))]
else:
return self.thisptr.data()[key]
def __setitem__(self, key, value):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
src_index = 0
for i in xrange (*key.indices(len(self))):
self.thisptr.data()[i] = value[src_index]
src_index = src_index + 1
else:
self.thisptr.data()[key] = value
def __len__ (self):
return 6
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return SpatialVector (address)
@classmethod
def fromPythonArray (cls, python_values):
return SpatialVector (0, python_values)
cdef class SpatialMatrix:
cdef crbdl.SpatialMatrix *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.SpatialMatrix()
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.SpatialMatrix*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "SpatialMatrix [{:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}, {:3.4f}]".format (
self.thisptr.data()[0], self.thisptr.data()[1], self.thisptr.data()[2],
self.thisptr.data()[3], self.thisptr.data()[4], self.thisptr.data()[5])
def __getitem__(self, key):
return self.thisptr.data()[key]
def __setitem__(self, key, value):
if isinstance( key, slice ) :
#Get the start, stop, and step from the slice
src_index = 0
for i in xrange (*key.indices(len(self))):
self.thisptr.data()[i] = value[src_index]
src_index = src_index + 1
else:
self.thisptr.data()[key] = value
def __len__ (self):
return 6
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return SpatialMatrix (address)
##############################
#
# Conversion Numpy <-> Eigen
#
##############################
# Vector3d
cdef crbdl.Vector3d NumpyToVector3d (np.ndarray[double, ndim=1, mode="c"] x):
cdef crbdl.Vector3d cx = crbdl.Vector3d()
for i in range (3):
cx[i] = x[i]
return cx
cdef np.ndarray Vector3dToNumpy (crbdl.Vector3d cx):
result = np.ndarray ((cx.rows()))
for i in range (cx.rows()):
result[i] = cx[i]
return result
cdef np.ndarray Matrix3dToNumpy (crbdl.Matrix3d cM):
result = np.ndarray ([3, 3])
for i in range (3):
for j in range (3):
result[i,j] = cM.coeff(i,j)
return result
# VectorNd
cdef crbdl.VectorNd NumpyToVectorNd (np.ndarray[double, ndim=1, mode="c"] x):
cdef crbdl.VectorNd cx = crbdl.VectorNd(x.shape[0])
for i in range (x.shape[0]):
cx[i] = x[i]
return cx
cdef np.ndarray VectorNdToNumpy (crbdl.VectorNd cx):
result = np.ndarray ((cx.rows()))
for i in range (cx.rows()):
result[i] = cx[i]
return result
# MatrixNd
cdef crbdl.MatrixNd NumpyToMatrixNd (np.ndarray[double, ndim=2, mode="c"] M):
cdef crbdl.MatrixNd cM = crbdl.MatrixNd(M.shape[0], M.shape[1])
for i in range (M.shape[0]):
for j in range (M.shape[1]):
(&(cM.coeff(i,j)))[0] = M[i,j]
return cM
cdef np.ndarray MatrixNdToNumpy (crbdl.MatrixNd cM):
result = np.ndarray ([cM.rows(), cM.cols()])
for i in range (cM.rows()):
for j in range (cM.cols()):
result[i,j] = cM.coeff(i,j)
return result
# SpatialVector
cdef np.ndarray SpatialVectorToNumpy (crbdl.SpatialVector cx):
result = np.ndarray ((cx.rows()))
for i in range (cx.rows()):
result[i] = cx[i]
return result
cdef crbdl.Quaternion NumpyToQuaternion (np.ndarray[double, ndim=1, mode="c"] x):
cdef crbdl.Quaternion cx = crbdl.Quaternion()
for i in range (3):
cx[i] = x[i]
return cx
cdef np.ndarray QuaternionToNumpy (crbdl.Quaternion cx):
result = np.ndarray ((cx.rows()))
for i in range (cx.rows()):
result[i] = cx[i]
return result
##############################
#
# Spatial Algebra Types
#
##############################
cdef class SpatialTransform:
cdef crbdl.SpatialTransform *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.SpatialTransform()
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.SpatialTransform*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "SpatialTransform E = [ [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}], [{:3.4f}, {:3.4f}, {:3.4f}] ], r = [{:3.4f}, {:3.4f}, {:3.4f}]".format (
self.thisptr.E.coeff(0,0), self.thisptr.E.coeff(0,1), self.thisptr.E.coeff(0,2),
self.thisptr.E.coeff(1,0), self.thisptr.E.coeff(1,1), self.thisptr.E.coeff(1,2),
self.thisptr.E.coeff(2,0), self.thisptr.E.coeff(2,1), self.thisptr.E.coeff(2,2),
self.thisptr.r[0], self.thisptr.r[1], self.thisptr.r[2])
property E:
""" Rotational part of the SpatialTransform. """
def __get__ (self):
result = np.ndarray ((3,3))
for i in range (3):
for j in range (3):
result[i,j] = self.thisptr.E.coeff(i,j)
return result
def __set__ (self, value):
for i in range (3):
for j in range (3):
(&(self.thisptr.E.coeff(i,j)))[0] = value[i,j]
property r:
""" Translational part of the SpatialTransform. """
def __get__ (self):
result = np.ndarray ((3))
for i in range (3):
result[i] = self.thisptr.r[i]
return result
def __set__ (self, value):
for i in range (3):
(&(self.thisptr.r[i]))[0] = value[i]
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return SpatialTransform (address)
cdef class SpatialRigidBodyInertia:
cdef crbdl.SpatialRigidBodyInertia *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.SpatialRigidBodyInertia()
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.SpatialRigidBodyInertia*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "rbdl.SpatialRigidBodyInertia (0x{:0x})".format(<uintptr_t><void *> self.thisptr)
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return SpatialRigidBodyInertia (address)
property m:
def __get__ (self):
return self.thisptr.m
def __set__ (self, value):
self.thisptr.m = value
property h:
""" Translational part of the SpatialRigidBodyInertia. """
def __get__ (self):
result = np.ndarray ((3))
for i in range (3):
result[i] = self.thisptr.h[i]
return result
def __set__ (self, value):
for i in range (3):
(&(self.thisptr.h[i]))[0] = value[i]
property Ixx:
def __get__ (self):
return self.thisptr.Ixx
def __set__ (self, value):
self.thisptr.Ixx = value
property Iyx:
def __get__ (self):
return self.thisptr.Iyx
def __set__ (self, value):
self.thisptr.Iyx = value
property Iyy:
def __get__ (self):
return self.thisptr.Iyy
def __set__ (self, value):
self.thisptr.Iyy = value
property Izx:
def __get__ (self):
return self.thisptr.Izx
def __set__ (self, value):
self.thisptr.Izx = value
property Izy:
def __get__ (self):
return self.thisptr.Izy
def __set__ (self, value):
self.thisptr.Izy = value
property Izz:
def __get__ (self):
return self.thisptr.Izz
def __set__ (self, value):
self.thisptr.Izz = value
##############################
#
# Rigid Multibody Types
#
##############################
cdef class Body:
cdef crbdl.Body *thisptr
cdef free_on_dealloc
def __cinit__(self, **kwargs):
cdef double c_mass
cdef crbdl.Vector3d c_com
cdef crbdl.Matrix3d c_inertia
cdef uintptr_t address=0
if "address" in kwargs.keys():
address=kwargs["address"]
mass = None
if "mass" in kwargs.keys():
mass=kwargs["mass"]
com = None
if "com" in kwargs.keys():
com=kwargs["com"]
inertia = None
if "inertia" in kwargs.keys():
inertia=kwargs["inertia"]
if address == 0:
self.free_on_dealloc = True
if (mass is not None) and (com is not None) and (inertia is not None):
c_mass = mass
for i in range (3):
c_com[i] = com[i]
for i in range (3):
for j in range (3):
(&(c_inertia.coeff(i,j)))[0] = inertia[i,j]
self.thisptr = new crbdl.Body(c_mass, c_com, c_inertia)
else:
self.thisptr = new crbdl.Body()
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.Body*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "rbdl.Body (0x{:0x})".format(<uintptr_t><void *> self.thisptr)
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return Body (address=address)
@classmethod
def fromMassComInertia(cls, double mass,
np.ndarray[double, ndim=1] com,
np.ndarray[double, ndim=2] inertia):
return Body (address=0, mass=mass, com=com, inertia=inertia)
# Properties
property mMass:
def __get__ (self):
return self.thisptr.mMass
def __set__ (self, value):
self.thisptr.mMass = value
property mCenterOfMass:
def __get__ (self):
result = np.ndarray ((3))
for i in range (3):
result[i] = self.thisptr.mCenterOfMass[i]
return result
def __set__ (self, value):
for i in range (3):
(&(self.thisptr.mCenterOfMass[i]))[0] = value[i]
property mInertia:
def __get__ (self):
result = np.ndarray ((3,3))
for i in range (3):
for j in range (3):
result[i,j] = self.thisptr.mInertia.coeff(i,j)
return result
def __set__ (self, value):
for i in range (3):
for j in range (3):
(&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j]
property mIsVirtual:
def __get__ (self):
return self.thisptr.mIsVirtual
def __set__ (self, value):
self.thisptr.mIsVirtual = value
cdef class FixedBody:
cdef crbdl.FixedBody *thisptr
cdef free_on_dealloc
def __cinit__(self, uintptr_t address=0):
if address == 0:
self.free_on_dealloc = True
self.thisptr = new crbdl.FixedBody()
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.FixedBody*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
return "rbdl.FixedBody (0x{:0x})".format(<uintptr_t><void *> self.thisptr)
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return FixedBody (address)
# Properties
property mMass:
def __get__ (self):
return self.thisptr.mMass
def __set__ (self, value):
self.thisptr.mMass = value
property mCenterOfMass:
def __get__ (self):
result = np.ndarray ((3))
for i in range (3):
result[i] = self.thisptr.mCenterOfMass[i]
return result
def __set__ (self, value):
for i in range (3):
(&(self.thisptr.mCenterOfMass[i]))[0] = value[i]
property mInertia:
def __get__ (self):
result = np.ndarray ((3,3))
for i in range (3):
for j in range (3):
result[i,j] = self.thisptr.mInertia.coeff(i,j)
return result
def __set__ (self, value):
for i in range (3):
for j in range (3):
(&(self.thisptr.mInertia.coeff(i,j)))[0] = value[i,j]
cdef enum JointType:
JointTypeUndefined = 0
JointTypeRevolute
JointTypePrismatic
JointTypeRevoluteX
JointTypeRevoluteY
JointTypeRevoluteZ
JointTypeSpherical
JointTypeEulerZYX
JointTypeEulerXYZ
JointTypeEulerYXZ
JointTypeTranslationXYZ
JointTypeFloatingBase
JointTypeFixed
JointTypeHelical
JointType1DoF
JointType2DoF
JointType3DoF
JointType4DoF
JointType5DoF
JointType6DoF
JointTypeCustom
cdef class Joint:
cdef crbdl.Joint *thisptr
cdef free_on_dealloc
joint_type_map = {
JointTypeUndefined: "JointTypeUndefined",
JointTypeRevolute: "JointTypeRevolute",
JointTypePrismatic: "JointTypePrismatic",
JointTypeRevoluteX: "JointTypeRevoluteX",
JointTypeRevoluteY: "JointTypeRevoluteY",
JointTypeRevoluteZ: "JointTypeRevoluteZ",
JointTypeSpherical: "JointTypeSpherical",
JointTypeEulerZYX: "JointTypeEulerZYX",
JointTypeEulerXYZ: "JointTypeEulerXYZ",
JointTypeEulerYXZ: "JointTypeEulerYXZ",
JointTypeTranslationXYZ: "JointTypeTranslationXYZ",
JointTypeFloatingBase: "JointTypeFloatingBase",
JointTypeFixed: "JointTypeFixed",
JointTypeHelical: "JointTypeHelical",
JointType1DoF: "JointType1DoF",
JointType2DoF: "JointType2DoF",
JointType3DoF: "JointType3DoF",
JointType4DoF: "JointType4DoF",
JointType5DoF: "JointType5DoF",
JointType6DoF: "JointType6DoF",
JointTypeCustom: "JointTypeCustom",
}
def _joint_type_from_str (self, joint_type_str):
if joint_type_str not in self.joint_type_map.values():
raise ValueError("Invalid JointType '" + str(joint_type_str) + "'!")
else:
for joint_type, joint_str in self.joint_type_map.iteritems():
if joint_str == joint_type_str:
return joint_type
def __cinit__(self, uintptr_t address=0, joint_type=-1):
if address == 0:
self.free_on_dealloc = True
if joint_type == -1:
self.thisptr = new crbdl.Joint()
else:
self.thisptr = new crbdl.Joint(self._joint_type_from_str(joint_type))
else:
self.free_on_dealloc = False
self.thisptr = <crbdl.Joint*>address
def __dealloc__(self):
if self.free_on_dealloc:
del self.thisptr
def __repr__(self):
joint_type_str = "JointTypeUndefined"
if self.thisptr.mJointType in self.joint_type_map.keys():
joint_type_str = self.joint_type_map[self.thisptr.mJointType]
return "rbdl.Joint (0x{:0x}), JointType: {:s}".format(<uintptr_t><void *> self.thisptr, joint_type_str)
# Constructors
@classmethod
def fromPointer(cls, uintptr_t address):
return Joint (address)
@classmethod
def fromJointType(cls, joint_type):
return Joint (0, joint_type)
@classmethod
def fromJointAxes(cls, axes):
assert (len(axes) > 0)
assert (len(axes[0]) == 6)
axes_count = len(axes)
joint_type = JointType1DoF + axes_count - 1
result = Joint (0, cls.joint_type_map[joint_type])
for i in range (axes_count):
result.setJointAxis(i, axes[i])
return result
property mDoFCount:
def __get__ (self):
return self.thisptr.mDoFCount
def __set__ (self, value):
self.thisptr.mDoFCount = value
property mJointType:
def __get__ (self):
return self.joint_type_map[self.thisptr.mJointType]
property q_index:
def __get__ (self):
return self.thisptr.q_index
def getJointAxis (self, index):
assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!"
return SpatialVectorToNumpy (self.thisptr.mJointAxes[index])
def setJointAxis (self, index, value):
assert index >= 0 and index < self.thisptr.mDoFCount, "Invalid joint axis index!"
for i in range (6):
(&(self.thisptr.mJointAxes[index][i]))[0] = value[i]
self.thisptr.mJointAxes[index][i]
cdef class Model
%VectorWrapperClassDefinitions(PARENT=Model)%
cdef class Model:
cdef crbdl.Model *thisptr
%VectorWrapperMemberDefinitions (PARENT=Model)%
def __cinit__(self):
self.thisptr = new crbdl.Model()
%VectorWrapperCInitCode (PARENT=Model)%
def __dealloc__(self):
del self.thisptr
def __repr__(self):
return "rbdl.Model (0x{:0x})".format(<uintptr_t><void *> self.thisptr)
def AddBody (self,
parent_id,
SpatialTransform joint_frame not None,
Joint joint not None,
Body body not None,
string body_name = b""):
return self.thisptr.AddBody (
parent_id,
joint_frame.thisptr[0],
joint.thisptr[0],
body.thisptr[0],
body_name
)
def AppendBody (self,
SpatialTransform joint_frame not None,
Joint joint not None,
Body body not None,
string body_name = b""):
return self.thisptr.AppendBody (
joint_frame.thisptr[0],
joint.thisptr[0],
body.thisptr[0],
body_name
)
def SetQuaternion (self,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] quat,
np.ndarray[double, ndim=1, mode="c"] q):
quat_wrap = Quaternion.fromPythonArray (quat)
q_wrap = VectorNd.fromPythonArray (q)
self.thisptr.SetQuaternion (body_id,
(<Quaternion>quat_wrap).thisptr[0],
(<VectorNd>q_wrap).thisptr[0])
for i in range(len(q)):
q[i] = q_wrap[i]
def GetQuaternion (self,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] q):
return QuaternionToNumpy (self.thisptr.GetQuaternion(body_id, NumpyToVectorNd (q)))
def GetBody (self, index):
return Body (address=<uintptr_t> &(self.thisptr.mBodies[index]))
def GetParentBodyId (self, index):
return self.thisptr.GetParentBodyId(index)
def GetBodyId (self, name):
return self.thisptr.GetBodyId(name)
def GetBodyName (self, index):
return self.thisptr.GetBodyName(index)
def IsBodyId (self, index):
return self.thisptr.IsBodyId(index)
def IsFixedBodyId (self, index):
return self.thisptr.IsFixedBodyId(index)
property dof_count:
def __get__ (self):
return self.thisptr.dof_count
property q_size:
def __get__ (self):
return self.thisptr.q_size
property qdot_size:
def __get__ (self):
return self.thisptr.qdot_size
property previously_added_body_id:
def __get__ (self):
return self.thisptr.previously_added_body_id
property gravity:
def __get__ (self):
return np.array ([
self.thisptr.gravity[0],
self.thisptr.gravity[1],
self.thisptr.gravity[2]
]
)
def __set__ (self, values):
for i in range (0,3):
self.thisptr.gravity[i] = values[i]
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=v, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=a, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=Joint, MEMBER=mJoints, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=S, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_J, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=v_J, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=c_J, PARENT=Model)%
property mJointUpdateOrder:
def __get__ (self):
return self.thisptr.mJointUpdateOrder
%VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_T, PARENT=Model)%
property mFixedJointCount:
def __get__ (self):
return self.thisptr.mFixedJointCount
# TODO
# multdof3_S
# multdof3_U
# multdof3_Dinv
# multdof3_u
property multdof3_w_index:
def __get__ (self):
return self.thisptr.multdof3_w_index
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=c, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialMatrix, MEMBER=IA, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=pA, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=U, PARENT=Model)%
# TODO
# d
# u
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=f, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialRigidBodyInertia, MEMBER=I, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialRigidBodyInertia, MEMBER=Ic, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialVector, MEMBER=hc, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_lambda, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=SpatialTransform, MEMBER=X_base, PARENT=Model)%
%VectorWrapperAddProperty (TYPE=FixedBody, MEMBER=mFixedBodies, PARENT=Model)%
property fixed_body_discriminator:
def __get__ (self):
return self.thisptr.fixed_body_discriminator
%VectorWrapperAddProperty (TYPE=Body, MEMBER=mBodies, PARENT=Model)%
##############################
#
# Constraint Types
#
##############################
cdef class ConstraintSet
%VectorWrapperClassDefinitions(PARENT=ConstraintSet)%
cdef class ConstraintSet:
cdef crbdl.ConstraintSet *thisptr
%VectorWrapperMemberDefinitions (PARENT=ConstraintSet)%
def __cinit__(self):
self.thisptr = new crbdl.ConstraintSet()
%VectorWrapperCInitCode (PARENT=ConstraintSet)%
def __dealloc__(self):
del self.thisptr
def __repr__(self):
return "rbdl.ConstraintSet (0x{:0x})".format(<uintptr_t><void *> self.thisptr)
def AddContactConstraint (self,
body_id not None,
body_point not None,
world_normal not None,
constraint_name = None,
normal_acceleration = 0.):
cdef crbdl.Vector3d c_body_point
cdef crbdl.Vector3d c_world_normal
cdef char* constraint_name_ptr
for i in range (3):
c_body_point[i] = body_point[i]
c_world_normal[i] = world_normal[i]
if constraint_name is None:
constraint_name_ptr = NULL
else:
constraint_name_ptr = constraint_name
return self.thisptr.AddContactConstraint (
body_id,
c_body_point,
c_world_normal,
constraint_name_ptr,
normal_acceleration
)
def AddLoopConstraint (self,
id_predecessor not None,
id_successor not None,
SpatialTransform X_predecessor not None,
SpatialTransform X_successor not None,
SpatialVector axis not None,
enable_stabilization = False,
stabilization_param = 0.1,
constraint_name = None):
cdef char* constraint_name_ptr
if constraint_name is None:
constraint_name_ptr = NULL
else:
constraint_name_ptr = constraint_name
return self.thisptr.AddLoopConstraint (
id_predecessor,
id_successor,
X_predecessor.thisptr[0],
X_successor.thisptr[0],
axis.thisptr[0],
enable_stabilization,
stabilization_param,
constraint_name_ptr)
def Bind (self, model):
return self.thisptr.Bind ((<Model>model).thisptr[0])
def size (self):
return self.thisptr.size()
def clear (self):
self.thisptr.clear()
property bound:
def __get__ (self):
return self.thisptr.bound
# %VectorWrapperAddProperty (TYPE=string, MEMBER=name, PARENT=ConstraintSet)%
%VectorWrapperAddProperty (TYPE=Vector3d, MEMBER=point, PARENT=ConstraintSet)%
%VectorWrapperAddProperty (TYPE=Vector3d, MEMBER=normal, PARENT=ConstraintSet)%
property force:
def __get__ (self):
return VectorNdToNumpy(self.thisptr.force)
# property acceleration:
# def __get__(self):
# return VectorNd.fromPointer (<uintptr_t> &(self.thisptr.acceleration)).toNumpy()
# def __set__(self, values):
# vec = VectorNd.fromPythonArray (values)
# self.thisptr.acceleration = <crbdl.VectorNd> (vec.thisptr[0])
##############################
#
# Kinematics.h
#
##############################
def UpdateKinematics(
Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] qddot
):
crbdl.UpdateKinematics(
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
NumpyToVectorNd (qddot)
)
def CalcBodyToBaseCoordinates (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
update_kinematics=True):
return Vector3dToNumpy (crbdl.CalcBodyToBaseCoordinates (
model.thisptr[0],
NumpyToVectorNd (q),
body_id,
NumpyToVector3d (body_point_position),
update_kinematics
))
def CalcBaseToBodyCoordinates (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
update_kinematics=True):
return Vector3dToNumpy (crbdl.CalcBaseToBodyCoordinates (
model.thisptr[0],
NumpyToVectorNd (q),
body_id,
NumpyToVector3d (body_point_position),
update_kinematics
))
def CalcBodyWorldOrientation (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
unsigned int body_id,
update_kinematics=True):
return Matrix3dToNumpy (crbdl.CalcBodyWorldOrientation (
model.thisptr[0],
NumpyToVectorNd (q),
body_id,
update_kinematics
))
def CalcPointVelocity (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
update_kinematics=True):
return Vector3dToNumpy (crbdl.CalcPointVelocity (
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
body_id,
NumpyToVector3d (body_point_position),
update_kinematics
))
def CalcPointAcceleration (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] qddot,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
update_kinematics=True):
return Vector3dToNumpy (crbdl.CalcPointAcceleration (
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
NumpyToVectorNd (qddot),
body_id,
NumpyToVector3d (body_point_position),
update_kinematics
))
def CalcPointVelocity6D (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
update_kinematics=True):
return SpatialVectorToNumpy (crbdl.CalcPointVelocity6D (
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
body_id,
NumpyToVector3d (body_point_position),
update_kinematics
))
def CalcPointAcceleration6D (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] qddot,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
update_kinematics=True):
return SpatialVectorToNumpy (crbdl.CalcPointAcceleration6D (
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
NumpyToVectorNd (qddot),
body_id,
NumpyToVector3d (body_point_position),
update_kinematics
))
def CalcPointJacobian (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
np.ndarray[double, ndim=2, mode="c"] G,
update_kinematics=True):
crbdl.CalcPointJacobianPtr (
model.thisptr[0],
<double*>q.data,
body_id,
NumpyToVector3d (body_point_position),
<double*>G.data,
update_kinematics
)
def CalcPointJacobian6D (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
np.ndarray[double, ndim=2, mode="c"] G,
update_kinematics=True):
crbdl.CalcPointJacobian6DPtr (
model.thisptr[0],
<double*>q.data,
body_id,
NumpyToVector3d (body_point_position),
<double*>G.data,
update_kinematics
)
def CalcBodySpatialJacobian(Model model,
np.ndarray[double, ndim=1, mode="c"] q,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point_position,
np.ndarray[double, ndim=2, mode="c"] G,
update_kinematics=True):
crbdl.CalcBodySpatialJacobianPtr(
model.thisptr[0],
<double*>q.data,
body_id,
<double*>G.data,
update_kinematics
)
##############################
#
# rbdl_utils.h
#
##############################
def CalcCenterOfMass (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] qddot,
np.ndarray[double, ndim=1, mode="c"] com,
np.ndarray[double, ndim=1, mode="c"] com_velocity=None,
np.ndarray[double, ndim=1, mode="c"] com_acceleration=None,
np.ndarray[double, ndim=1, mode="c"] angular_momentum=None,
np.ndarray[double, ndim=1, mode="c"] change_of_angular_momentum=None,
update_kinematics=True):
cdef double cmass
cdef crbdl.Vector3d c_com = crbdl.Vector3d()
cdef crbdl.Vector3d* c_com_vel_ptr # = crbdl.Vector3d()
cdef crbdl.Vector3d* c_com_accel_ptr # = crbdl.Vector3d()
cdef crbdl.Vector3d* c_ang_momentum_ptr # = crbdl.Vector3d()
cdef crbdl.Vector3d* c_change_of_ang_momentum_ptr # = crbdl.Vector3d()
c_com_vel_ptr = <crbdl.Vector3d*> NULL
c_com_accel_ptr = <crbdl.Vector3d*> NULL
c_ang_momentum_ptr = <crbdl.Vector3d*> NULL
c_change_of_ang_momentum_ptr = <crbdl.Vector3d*> NULL
if com_velocity is not None:
c_com_vel_ptr = new crbdl.Vector3d()
if com_acceleration is not None:
c_com_accel_ptr = new crbdl.Vector3d()
if angular_momentum is not None:
c_ang_momentum_ptr = new crbdl.Vector3d()
if change_of_angular_momentum is not None:
c_change_of_ang_momentum_ptr = new crbdl.Vector3d()
cdef crbdl.VectorNd qddot_vectornd
if qddot is not None:
qddot_vectornd = NumpyToVectorNd(qddot)
cmass = 0.0
crbdl.CalcCenterOfMass (
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
<crbdl.VectorNd*> NULL if qddot is None else &qddot_vectornd,
cmass,
c_com,
c_com_vel_ptr,
c_com_accel_ptr,
c_ang_momentum_ptr,
c_change_of_ang_momentum_ptr,
update_kinematics)
assert com is not None, "Parameter com for call to CalcCenterOfMass() is not provided (value is 'None')."
com[0] = c_com[0]
com[1] = c_com[1]
com[2] = c_com[2]
if com_velocity is not None:
com_velocity[0] = c_com_vel_ptr.data()[0]
com_velocity[1] = c_com_vel_ptr.data()[1]
com_velocity[2] = c_com_vel_ptr.data()[2]
del c_com_vel_ptr
if com_acceleration is not None:
com_acceleration[0] = c_com_accel_ptr.data()[0]
com_acceleration[1] = c_com_accel_ptr.data()[1]
com_acceleration[2] = c_com_accel_ptr.data()[2]
del c_com_accel_ptr
if angular_momentum is not None:
angular_momentum[0] = c_ang_momentum_ptr.data()[0]
angular_momentum[1] = c_ang_momentum_ptr.data()[1]
angular_momentum[2] = c_ang_momentum_ptr.data()[2]
del c_ang_momentum_ptr
if change_of_angular_momentum is not None:
change_of_angular_momentum[0] = c_change_of_ang_momentum_ptr.data()[0]
change_of_angular_momentum[1] = c_change_of_ang_momentum_ptr.data()[1]
change_of_angular_momentum[2] = c_change_of_ang_momentum_ptr.data()[2]
del c_change_of_ang_momentum_ptr
return cmass
def CalcZeroMomentPoint (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] qddot,
np.ndarray[double, ndim=1, mode="c"] zmp,
np.ndarray[double, ndim=1, mode="c"] normal=None,
np.ndarray[double, ndim=1, mode="c"] point=None,
update_kinematics=True):
cdef crbdl.Vector3d c_normal = crbdl.Vector3d()
cdef crbdl.Vector3d c_point = crbdl.Vector3d()
cdef crbdl.Vector3d* c_zmp_ptr# = crbdl.Vector3d()
c_zmp_ptr = new crbdl.Vector3d()
if normal is not None:
c_normal[0] = normal[0]
c_normal[1] = normal[1]
c_normal[2] = normal[2]
else:
c_normal[0] = 0
c_normal[1] = 0
c_normal[2] = 1
if point is not None:
c_point[0] = point[0]
c_point[1] = point[1]
c_point[2] = point[2]
crbdl.CalcZeroMomentPoint (
model.thisptr[0],
NumpyToVectorNd (q),
NumpyToVectorNd (qdot),
NumpyToVectorNd (qddot),
c_zmp_ptr,
c_normal,
c_point,
update_kinematics)
zmp[0] = c_zmp_ptr.data()[0]
zmp[1] = c_zmp_ptr.data()[1]
zmp[2] = c_zmp_ptr.data()[2]
return zmp
##############################
#
# Dynamics.h
#
##############################
def InverseDynamics (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] qddot,
np.ndarray[double, ndim=1, mode="c"] tau):
crbdl.InverseDynamicsPtr (model.thisptr[0],
<double*>q.data,
<double*>qdot.data,
<double*>qddot.data,
<double*>tau.data,
NULL
)
def NonlinearEffects (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] tau):
crbdl.NonlinearEffectsPtr (model.thisptr[0],
<double*>q.data,
<double*>qdot.data,
<double*>tau.data
)
def CompositeRigidBodyAlgorithm (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=2, mode="c"] H,
update_kinematics=True):
crbdl.CompositeRigidBodyAlgorithmPtr (model.thisptr[0],
<double*>q.data,
<double*>H.data,
update_kinematics);
def ForwardDynamics (Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] tau,
np.ndarray[double, ndim=1, mode="c"] qddot):
crbdl.ForwardDynamicsPtr (model.thisptr[0],
<double*>q.data,
<double*>qdot.data,
<double*>tau.data,
<double*>qddot.data,
NULL
)
##############################
#
# Constraints.h
#
##############################
def ForwardDynamicsConstraintsDirect (
Model model,
np.ndarray[double, ndim=1, mode="c"] q,
np.ndarray[double, ndim=1, mode="c"] qdot,
np.ndarray[double, ndim=1, mode="c"] tau,
ConstraintSet CS,
np.ndarray[double, ndim=1, mode="c"] qddot):
crbdl.ForwardDynamicsConstraintsDirectPtr (
model.thisptr[0],
<double*>q.data,
<double*>qdot.data,
<double*>tau.data,
CS.thisptr[0],
<double*>qddot.data
)
##############################
#
# Utilities
#
##############################
def loadModel (
filename,
**kwargs
):
verbose = False
if "verbose" in kwargs.keys():
verbose=kwargs["verbose"]
floating_base = False
if "floating_base" in kwargs.keys():
floating_base=kwargs["floating_base"]
result = Model()
if crbdl.rbdl_loadmodel (filename, result.thisptr, floating_base, verbose):
return result
print ("Error loading model {}!".format (filename))
return None
3rdParty/rbdl/python/rbdl_loadmodel.cc deleted 100644 → 0
View file @ a5f65699
#include <rbdl/rbdl.h>
#ifdef RBDL_BUILD_ADDON_LUAMODEL
#include <addons/luamodel/luamodel.h>
#endif
#ifdef RBDL_BUILD_ADDON_URDFREADER
#include <addons/urdfreader/urdfreader.h>
#endif
#include <string>
#include <ctype.h>
using namespace RigidBodyDynamics;
using namespace std;
bool rbdl_loadmodel (const char* filename, Model* model, bool floating_base=false, bool verbose=false) {
string fname (filename);
for (size_t i = 0; i < fname.size(); i++) {
fname[i] = tolower(fname[i]);
}
bool result = false;
if (fname.substr (fname.size() - 4, 4) == ".lua") {
#ifdef RBDL_BUILD_ADDON_LUAMODEL
result = Addons::LuaModelReadFromFile (filename, model, verbose);
#else
cerr << "Error: RBDL Addon LuaModel not enabled!" << endl;
#endif
} else if (fname.substr (fname.size() - 5, 5) == ".urdf") {
#ifdef RBDL_BUILD_ADDON_URDFREADER
result = Addons::URDFReadFromFile (filename, model, floating_base, verbose);
#else
cerr << "Error: RBDL Addon URDFReader not enabled!" << endl;
#endif
} else {
cerr << "Error: Cannot identify model type from filename '" << filename << "'!" << endl;
}
return result;
}
3rdParty/rbdl/python/rbdl_ptr_functions.h deleted 100644 → 0
View file @ a5f65699
/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2011-2015 Martin Felis <martin@fysx.org>
*
* Licensed under the zlib license. See LICENSE for more details.
*
* This file defines functions that allows calling of the RBDL algorithms
* by providing input and output as raw double arrays. It eliminates the
* need of copying from Numpy values into temporary RBDL (C++) vectors and
* matrices. However it requires C++11 and must be compiled with -std=c++11
* (or -std=c++0x on older compilers).
*/
#include <rbdl/rbdl_math.h>
#include <rbdl/Dynamics.h>
namespace RigidBodyDynamics {
namespace Math {
// PTR_DATA_ROW_MAJOR :
// Specifies whether the data that is provided via raw double pointers is
// stored as row major. Eigen uses column major by default and therefore
// this has to be properly mapped.
#define PTR_DATA_ROW_MAJOR 1
#ifdef RBDL_USE_SIMPLE_MATH
typedef VectorNd VectorNdRef;
typedef MatrixNd MatrixNdRef;
#else
typedef Eigen::Ref<Eigen::VectorXd> VectorNdRef;
#ifdef PTR_DATA_ROW_MAJOR
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixNdRowMaj;
typedef Eigen::Ref<MatrixNdRowMaj> MatrixNdRef;
#else
typedef Eigen::Ref<Eigen::MatrixXd> MatrixNdRef;
#endif
#endif
RBDL_DLLAPI inline VectorNdRef VectorFromPtr (double *ptr, unsigned int n) {
#ifdef RBDL_USE_SIMPLE_MATH
return SimpleMath::Map<VectorNd> (ptr, n, 1);
#elif defined EIGEN_CORE_H
return Eigen::Map<VectorNd> (ptr, n, 1);
#else
std::cerr << __func__ << " not defined for used math library!" << std::endl;
abort();
return VectorNd::Constant (1,1./0.);
#endif
}
RBDL_DLLAPI inline MatrixNdRef MatrixFromPtr (double *ptr, unsigned int rows, unsigned int cols, bool row_major = true) {
#ifdef RBDL_USE_SIMPLE_MATH
return SimpleMath::Map<MatrixNd> (ptr, rows, cols);
#elif defined EIGEN_CORE_H
#ifdef PTR_DATA_ROW_MAJOR
return Eigen::Map<MatrixNdRowMaj> (ptr, rows, cols);
#else
return Eigen::Map<MatrixNd> (ptr, rows, cols);
#endif
#else
std::cerr << __func__ << " not defined for used math library!" << std::endl;
abort();
return MatrixNd::Constant (1,1, 1./0.);
#endif
}
}
RBDL_DLLAPI
void UpdateKinematicsCustomPtr (Model &model,
const double *q_ptr,
const double *qdot_ptr,
const double *qddot_ptr
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
unsigned int i;
if (q_ptr) {
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
for (i = 1; i < model.mBodies.size(); i++) {
unsigned int lambda = model.lambda[i];
VectorNd QDot_zero (VectorNd::Zero (model.q_size));
jcalc (model, i, (Q), QDot_zero);
model.X_lambda[i] = model.X_J[i] * model.X_T[i];
if (lambda != 0) {
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
} else {
model.X_base[i] = model.X_lambda[i];
}
}
}
if (qdot_ptr) {
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
VectorNdRef QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.q_size);
for (i = 1; i < model.mBodies.size(); i++) {
unsigned int lambda = model.lambda[i];
jcalc (model, i, Q, QDot);
if (lambda != 0) {
model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
} else {
model.v[i] = model.v_J[i];
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
}
// LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl;
}
}
if (qddot_ptr) {
VectorNdRef QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.q_size);
for (i = 1; i < model.mBodies.size(); i++) {
unsigned int q_index = model.mJoints[i].q_index;
unsigned int lambda = model.lambda[i];
if (lambda != 0) {
model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
} else {
model.a[i] = model.c[i];
}
if (model.mJoints[i].mDoFCount == 3) {
Vector3d omegadot_temp ((QDDot)[q_index], (QDDot)[q_index + 1], (QDDot)[q_index + 2]);
model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp;
} else {
model.a[i] = model.a[i] + model.S[i] * (QDDot)[q_index];
}
}
}
}
RBDL_DLLAPI
void CalcPointJacobianPtr (
Model &model,
const double *q_ptr,
unsigned int body_id,
const Math::Vector3d &point_position,
double * G_ptr,
bool update_kinematics
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
// update the Kinematics if necessary
if (update_kinematics) {
UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL);
}
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
MatrixNdRef G = MatrixFromPtr(const_cast<double*>(G_ptr), 3, model.qdot_size);
SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false));
assert (G.rows() == 3 && G.cols() == model.qdot_size );
unsigned int reference_body_id = body_id;
if (model.IsFixedBodyId(body_id)) {
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
}
unsigned int j = reference_body_id;
// e[j] is set to 1 if joint j contributes to the jacobian that we are
// computing. For all other joints the column will be zero.
while (j != 0) {
unsigned int q_index = model.mJoints[j].q_index;
if (model.mJoints[j].mDoFCount == 3) {
G.block(0, q_index, 3, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(3,0,3,3);
} else {
G.block(0,q_index, 3, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(3,0,3,1);
}
j = model.lambda[j];
}
}
RBDL_DLLAPI
void CalcPointJacobian6DPtr (
Model &model,
const double *q_ptr,
unsigned int body_id,
const Math::Vector3d &point_position,
double *G_ptr,
bool update_kinematics
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
// update the Kinematics if necessary
if (update_kinematics) {
UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL);
}
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
MatrixNdRef G = MatrixFromPtr(const_cast<double*>(G_ptr), 6, model.qdot_size);
SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false));
assert (G.rows() == 6 && G.cols() == model.qdot_size );
unsigned int reference_body_id = body_id;
if (model.IsFixedBodyId(body_id)) {
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
}
unsigned int j = reference_body_id;
while (j != 0) {
unsigned int q_index = model.mJoints[j].q_index;
if (model.mJoints[j].mDoFCount == 3) {
G.block(0, q_index, 6, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(0,0,6,3);
} else {
G.block(0,q_index, 6, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(0,0,6,1);
}
j = model.lambda[j];
}
}
RBDL_DLLAPI
void CalcBodySpatialJacobianPtr (
Model &model,
const double *q_ptr,
unsigned int body_id,
double *G_ptr,
bool update_kinematics
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
// update the Kinematics if necessary
if (update_kinematics) {
UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL);
}
MatrixNdRef G = MatrixFromPtr(const_cast<double*>(G_ptr), 6, model.q_size);
assert (G.rows() == 6 && G.cols() == model.qdot_size );
unsigned int reference_body_id = body_id;
SpatialTransform base_to_body;
if (model.IsFixedBodyId(body_id)) {
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
base_to_body = model.mFixedBodies[fbody_id].mParentTransform * model.X_base[reference_body_id];
} else {
base_to_body = model.X_base[reference_body_id];
}
unsigned int j = reference_body_id;
while (j != 0) {
unsigned int q_index = model.mJoints[j].q_index;
if (model.mJoints[j].mDoFCount == 3) {
G.block(0,q_index,6,3) = (base_to_body * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j];
} else {
G.block(0,q_index,6,1) = base_to_body.apply(model.X_base[j].inverse().apply(model.S[j]));
}
j = model.lambda[j];
}
}
RBDL_DLLAPI
void InverseDynamicsPtr (
Model &model,
const double *q_ptr,
const double *qdot_ptr,
const double *qddot_ptr,
const double *tau_ptr,
std::vector<Math::SpatialVector> *f_ext
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
VectorNdRef QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.q_size);
VectorNdRef QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.q_size);
VectorNdRef Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.q_size);
// Reset the velocity of the root body
model.v[0].setZero();
model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
unsigned int q_index = model.mJoints[i].q_index;
unsigned int lambda = model.lambda[i];
jcalc (model, i, Q, QDot);
if (lambda != 0) {
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
} else {
model.X_base[i] = model.X_lambda[i];
}
model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
if (model.mJoints[i].mDoFCount == 3) {
model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.multdof3_S[i] * Vector3d (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]);
} else {
model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.S[i] * QDDot[q_index];
}
if (!model.mBodies[i].mIsVirtual) {
model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]);
} else {
model.f[i].setZero();
}
if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero())
model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
}
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
if (model.mJoints[i].mDoFCount == 3) {
Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i];
} else {
Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]);
}
if (model.lambda[i] != 0) {
model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
}
}
}
RBDL_DLLAPI
void NonlinearEffectsPtr (
Model &model,
const double *q_ptr,
const double *qdot_ptr,
const double *tau_ptr
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
VectorNdRef QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.q_size);
VectorNdRef Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.q_size);
SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
// Reset the velocity of the root body
model.v[0].setZero();
model.a[0] = spatial_gravity;
for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) {
jcalc (model, model.mJointUpdateOrder[i], Q, QDot);
}
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
if (model.lambda[i] == 0) {
model.v[i] = model.v_J[i];
model.a[i] = model.X_lambda[i].apply(spatial_gravity);
} else {
model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i];
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i];
}
if (!model.mBodies[i].mIsVirtual) {
model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]);
} else {
model.f[i].setZero();
}
}
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
if (model.mJoints[i].mDoFCount == 3) {
Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i];
} else {
Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]);
}
if (model.lambda[i] != 0) {
model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
}
}
}
RBDL_DLLAPI
inline void CompositeRigidBodyAlgorithmPtr (
Model& model,
const double *q_ptr,
double *H_ptr,
bool update_kinematics = true
) {
using namespace RigidBodyDynamics::Math;
VectorNdRef&& Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
MatrixNdRef&& H = MatrixFromPtr(H_ptr, model.qdot_size, model.qdot_size);
assert (H.rows() == model.dof_count && H.cols() == model.dof_count);
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
if (update_kinematics) {
jcalc_X_lambda_S (model, i, Q);
}
model.Ic[i] = model.I[i];
}
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
if (model.lambda[i] != 0) {
model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]);
}
unsigned int dof_index_i = model.mJoints[i].q_index;
if (model.mJoints[i].mDoFCount == 3) {
Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i];
H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63;
unsigned int j = i;
unsigned int dof_index_j = dof_index_i;
while (model.lambda[j] != 0) {
F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63);
j = model.lambda[j];
dof_index_j = model.mJoints[j].q_index;
if (model.mJoints[j].mDoFCount == 3) {
Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]);
H.block<3,3>(dof_index_i,dof_index_j) = H_temp2;
H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
} else {
Vector3d H_temp2 = F_63.transpose() * (model.S[j]);
H.block<3,1>(dof_index_i,dof_index_j) = H_temp2;
H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
}
}
} else {
SpatialVector F = model.Ic[i] * model.S[i];
H(dof_index_i, dof_index_i) = model.S[i].dot(F);
unsigned int j = i;
unsigned int dof_index_j = dof_index_i;
while (model.lambda[j] != 0) {
F = model.X_lambda[j].applyTranspose(F);
j = model.lambda[j];
dof_index_j = model.mJoints[j].q_index;
if (model.mJoints[j].mDoFCount == 3) {
Vector3d H_temp2 = (F.transpose() * model.multdof3_S[j]).transpose();
LOG << F.transpose() << std::endl << model.multdof3_S[j] << std::endl;
LOG << H_temp2.transpose() << std::endl;
H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose();
H.block<3,1>(dof_index_j,dof_index_i) = H_temp2;
} else {
H(dof_index_i,dof_index_j) = F.dot(model.S[j]);
H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j);
}
}
}
}
}
RBDL_DLLAPI
void ForwardDynamicsPtr (
Model &model,
const double *q_ptr,
const double *qdot_ptr,
const double *tau_ptr,
const double *qddot_ptr,
std::vector<Math::SpatialVector> *f_ext
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
VectorNdRef&& Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
VectorNdRef&& QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.q_size);
VectorNdRef&& QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.q_size);
VectorNdRef&& Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.q_size);
SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
unsigned int i = 0;
LOG << "Q = " << Q.transpose() << std::endl;
LOG << "QDot = " << QDot.transpose() << std::endl;
LOG << "Tau = " << Tau.transpose() << std::endl;
LOG << "---" << std::endl;
// Reset the velocity of the root body
model.v[0].setZero();
for (i = 1; i < model.mBodies.size(); i++) {
unsigned int lambda = model.lambda[i];
jcalc (model, i, Q, QDot);
if (lambda != 0)
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
else
model.X_base[i] = model.X_lambda[i];
model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i];
/*
LOG << "X_J (" << i << "):" << std::endl << X_J << std::endl;
LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl;
LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl;
LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl;
LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl;
LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl;
*/
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
model.I[i].setSpatialMatrix (model.IA[i]);
model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]);
if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) {
LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl;
model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
}
}
// ClearLogOutput();
LOG << "--- first loop ---" << std::endl;
for (i = model.mBodies.size() - 1; i > 0; i--) {
unsigned int q_index = model.mJoints[i].q_index;
if (model.mJoints[i].mDoFCount == 3) {
model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
#ifdef EIGEN_CORE_H
model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval();
#else
model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse();
#endif
Vector3d tau_temp (Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]);
model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i];
// LOG << "multdof3_u[" << i << "] = " << model.multdof3_u[i].transpose() << std::endl;
unsigned int lambda = model.lambda[i];
if (lambda != 0) {
SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose();
SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i];
#ifdef EIGEN_CORE_H
model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
#else
model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
#endif
LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl;
}
} else {
model.U[i] = model.IA[i] * model.S[i];
model.d[i] = model.S[i].dot(model.U[i]);
model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
// LOG << "u[" << i << "] = " << model.u[i] << std::endl;
unsigned int lambda = model.lambda[i];
if (lambda != 0) {
SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i];
#ifdef EIGEN_CORE_H
model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
#else
model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
#endif
LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl;
}
}
}
// ClearLogOutput();
model.a[0] = spatial_gravity * -1.;
for (i = 1; i < model.mBodies.size(); i++) {
unsigned int q_index = model.mJoints[i].q_index;
unsigned int lambda = model.lambda[i];
SpatialTransform X_lambda = model.X_lambda[i];
model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
if (model.mJoints[i].mDoFCount == 3) {
Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
QDDot[q_index] = qdd_temp[0];
QDDot[q_index + 1] = qdd_temp[1];
QDDot[q_index + 2] = qdd_temp[2];
model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
} else {
QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
}
}
LOG << "QDDot = " << QDDot.transpose() << std::endl;
}
RBDL_DLLAPI
void ForwardDynamicsConstraintsDirectPtr (
Model &model,
const double *q_ptr,
const double *qdot_ptr,
const double *tau_ptr,
ConstraintSet &CS,
double *qddot_ptr
) {
LOG << "-------- " << __func__ << " --------" << std::endl;
using namespace RigidBodyDynamics::Math;
VectorNdRef&& Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
VectorNdRef&& QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.q_size);
VectorNdRef&& QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.q_size);
VectorNdRef&& Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.q_size);
// create copy of non-const accelerations
VectorNd QDDot_dummy = QDDot;
LOG << "Q = " << Q.transpose() << std::endl;
LOG << "QDot = " << QDot.transpose() << std::endl;
LOG << "Tau = " << Tau.transpose() << std::endl;
LOG << "QDDot = " << QDDot.transpose() << std::endl;
LOG << "QDDot_dummy = " << QDDot_dummy.transpose() << std::endl;
// calling non-pointer version
ForwardDynamicsConstraintsDirect (
model, Q, QDot, Tau, CS, QDDot_dummy
);
for (int i = 0; i < model.q_size; ++i) {
QDDot[i] = QDDot_dummy[i];
}
LOG << "QDDot = " << QDDot.transpose() << std::endl;
LOG << "QDDot_dummy = " << QDDot_dummy.transpose() << std::endl;
LOG << "---" << std::endl;
}
}
3rdParty/rbdl/python/setup.py.cmake deleted 100755 → 0
View file @ a5f65699
#!/usr/bin/env python
from distutils.core import setup
from distutils.extension import Extension
from Cython.Distutils import build_ext
from Cython.Build import cythonize
import os
import numpy as np
BASEDIR = os.path.dirname(os.path.abspath(__file__))
extra_params = {}
extra_params['include_dirs'] = [
'/usr/include',
BASEDIR,
np.get_include(),
'@RBDL_INCLUDE_DIR@',
'@EIGEN3_INCLUDE_DIR@',
'@CMAKE_CURRENT_SOURCE_DIR@',
'@RBDL_SOURCE_DIR@/include',
'@RBDL_BINARY_DIR@/include',
'/usr/include/eigen3/'
]
extra_params['language'] = 'c++'
extra_params['extra_compile_args'] = ["-O3", "-Wno-unused-variable", "-std=c++11"]
extra_params['libraries'] = ['rbdl']
extra_params['library_dirs'] = [
'${CMAKE_CURRENT_BINARY_DIR}/../',
'${CMAKE_INSTALL_PREFIX}/lib/',
'/usr/lib',
BASEDIR
]
extra_params['extra_link_args'] = [
"-Wl,-O1",
"-Wl,--as-needed",
]
if os.name == 'posix':
extra_params['runtime_library_dirs'] = extra_params['library_dirs']
ext_modules = [
Extension("rbdl", ["crbdl.pxd", "rbdl.pyx"], **extra_params),
]
setup(
name='rbdl',
author='Martin Felis',
author_email='martin@fysx.org',
description='Python wrapper for RBDL - the Rigid Body Dynamics Library',
license='zlib',
version='${RBDL_VERSION_MAJOR}.${RBDL_VERSION_MINOR}.${RBDL_VERSION_PATCH}',
url='http://rbdl.bitbucket.org/',
cmdclass={'build_ext': build_ext},
ext_modules=cythonize(ext_modules),
)