rbdlsim/3rdparty/rbdl/python/rbdl-wrapper.pyx

1654 lines
50 KiB
Cython
Raw Normal View History

2020-10-03 22:55:14 +02:00
#cython: c_string_type=unicode, c_string_encoding=default, 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=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])
cdef class InverseKinematicsConstraintSet:
cdef crbdl.InverseKinematicsConstraintSet *thisptr
# cdef _ConstraintSet_point_Vector3d_VectorWrapper point
# cdef _ConstraintSet_normal_Vector3d_VectorWrapper normal
def __cinit__(self):
self.thisptr = new crbdl.InverseKinematicsConstraintSet()
# self.body_points = _ConstraintSet_body_points_Vector3d_VectorWrapper (<uintptr_t> self.thisptr)
# self.target_positions = _ConstraintSet_target_positions_Vector3d_VectorWrapper (<uintptr_t> self.thisptr)
# self.target_orientations = _ConstraintSet_target_orientations_Vector3d_VectorWrapper (<uintptr_t> self.thisptr)
def __dealloc__(self):
del self.thisptr
# def __repr__(self):
# return "rbdl.ConstraintSet (0x{:0x})".format(<uintptr_t><void *> self.thisptr)
def AddPointConstraint (self,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point,
np.ndarray[double, ndim=1, mode="c"] target_pos
):
cdef crbdl.Vector3d c_body_point
cdef crbdl.Vector3d c_target_pos
for i in range (3):
c_body_point[i] = body_point[i]
c_target_pos[i] = target_pos[i]
return self.thisptr.AddPointConstraint (
body_id,
c_body_point,
c_target_pos
)
def AddOrientationConstraint (self,
unsigned int body_id,
np.ndarray[double, ndim=2, mode="c"] target_orientation
):
cdef crbdl.Vector3d c_body_point
cdef crbdl.Matrix3d c_target_orientation
for i in range (3):
for j in range (3):
(&(c_target_orientation.coeff(i,j)))[0] = target_orientation[i, j]
return self.thisptr.AddOrientationConstraint (
body_id,
c_target_orientation
)
def AddFullConstraint (self,
unsigned int body_id,
np.ndarray[double, ndim=1, mode="c"] body_point,
np.ndarray[double, ndim=1, mode="c"] target_pos,
np.ndarray[double, ndim=2, mode="c"] target_orientation
):
cdef crbdl.Vector3d c_body_point
cdef crbdl.Vector3d c_target_pos
cdef crbdl.Matrix3d c_target_orientation
for i in range (3):
c_body_point[i] = body_point[i]
c_target_pos[i] = target_pos[i]
for j in range (3):
(&(c_target_orientation.coeff(i,j)))[0] = target_orientation[i, j]
return self.thisptr.AddFullConstraint (
body_id,
c_body_point,
c_target_pos,
c_target_orientation
)
property e:
def __get__ (self):
return VectorNd.fromPointer (<uintptr_t> &(self.thisptr.e)).toNumpy()
# property damper:
# def __get__ (self):
# return self.thisptr.damper
# def __set__ (self, value):
# self.thisptr.damper = value
def InverseKinematics (
Model model,
np.ndarray[double, ndim=1, mode="c"] Qinit,
InverseKinematicsConstraintSet CS
):
"""Compute solution for inverse kinematics for given constraint set."""
cdef crbdl.VectorNd Qres
Qres.resize(model.dof_count)
res = np.zeros(model.dof_count)
# copy over data
# FIXME use memory views
for i in range(model.dof_count):
Qres[i] = 0.0
ret = crbdl.InverseKinematics (
model.thisptr[0],
NumpyToVectorNd (Qinit),
CS.thisptr[0],
Qres
)
# copy over data
# FIXME use memory views
for i in range(model.dof_count):
res[i] = Qres[i]
return res
##############################
#
# 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