Commits

Martin Felis  committed 67d960b

[matlab] added ForwardDynamics, InverseDynamics, SetGravity, and GetGravity

  • Participants
  • Parent commits 6fe9757
  • Branches dev, matlab

Comments (0)

Files changed (8)

File wrapper/Makefile

+RBDL_SRC_PATH="../src"
+RBDL_BINARY_PATH="../Debug/"
+RBDL_CONFIG_H_PATH="${RBDL_BINARY_PATH}/src"
+
+# No configuration should be required below this line
+
 SRC_FILES := $(patsubst %.cc,%.mex,$(wildcard *.cc))
 
 all: $(SRC_FILES)
 
 %.mex: %.cc
-	mkoctfile --mex -I../src/ -I../Debug/src -L../Debug -Wl,-rpath,../Debug -lrbdl $<
+	mkoctfile --mex -I${RBDL_SRC_PATH} -I${RBDL_CONFIG_H_PATH} -L${RBDL_BINARY_PATH} -Wl,-rpath,${RBDL_BINARY_PATH} -lrbdl $<
 
 clean:
 	rm -f *.mex *.o

File wrapper/README

+This directory constains wrappers for some of the RBDL functions. This is
+more of a proof of concept but adding wrappers for the complete library should
+be straight forward.
+
+If anyone is interested in theses wrappers feel free to contact me or send
+patches. I would be happy to assist.
+
+You may have to modify the Makefile to adjust the *_PATH settings to be able to
+compile it.
+
+See example.m for usage and API overview.

File wrapper/example.m

 model = rbdlCreateModel()
 
 joint_frame.r = [1.3, 5.1, 1.4];
-joint_frame.E = [[0.0, 0.1, 0.2]; [1.0, 1.1, 1.2]; [2.0, 2.1, 2.2]];
+joint_frame.E = [[1.0, 0.0, 0.0]; [0.0, 1.0, 0.0]; [0.0, 0.0, 1.0]];
 joint = [ 
 	[0., 0., 0., 1., 0., 0.]; 
 	[0., 0., 0., 0., 1., 0.];
 
 body.mass = 93.;
 body.com = [1.1, 0.2, 0.3];
-body.inertia = [[0.0, 0.1, 0.2]; [1.0, 1.1, 1.2]; [2.0, 2.1, 2.2]]
+body.inertia = [[1.0, 0.1, 0.2]; [0.1, 2., 0.3]; [0.2, 0.3, 3.]]
+
+rbdlSetGravity (model, [0., 9.81, 0.]);
+gravity = rbdlGetGravity (model)
 
 rbdlAddBody (model, 0, joint_frame, joint, body, "test_body");
 rbdlAppendBody (model, joint_frame, joint, body, "test_body_appended");
 
 dof_count = rbdlGetDofCount(model)
 
+q = ones(dof_count, 1);
+qdot = ones(dof_count, 1);
+qddot = ones(dof_count, 1);
+tau = ones(dof_count, 1);
+
+qddot = rbdlForwardDynamics (model, q, qdot, tau)
+tau = rbdlInverseDynamics (model, q, qdot, qddot)
+
 rbdlDestroyModel(model)

File wrapper/rbdlForwardDynamics.cc

+#include "mex.h"
+#include "rbdl_mex.h"
+#include "ObjectHandle.h"
+
+#include <string>
+
+using namespace RigidBodyDynamics;
+
+void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
+	if (nrhs != 4) {
+		mexErrMsgTxt ("usage: qddot = rbdlForwardDynamics(model, q, qdot, tau");
+		return;
+	}
+
+	ObjectHandle<Model> *handle = ObjectHandle<Model>::from_mex_handle(prhs[0]);
+	Model *model = &(handle->get_object());
+
+	if (!mxIsNumeric(prhs[1])) {
+		mexErrMsgTxt("Argument 2 has to be the parent id (i.e. a number!");
+		return;
+	}
+
+	RigidBodyDynamics::Math::VectorNd q (model->dof_count), 
+					 qdot(model->dof_count), 
+					 qddot (model->dof_count),
+					 tau (model->dof_count);
+
+	if (!GetVectorNd (prhs[1], q)) {
+		mexErrMsgTxt ("Error parsing argument 1 (q)");
+		return;
+	}
+
+	if (!GetVectorNd (prhs[2], qdot)) {
+		mexErrMsgTxt ("Error parsing argument 2 (qdot)");
+		return;
+	}
+
+	if (!GetVectorNd (prhs[3], tau)) {
+		mexErrMsgTxt ("Error parsing argument 3 (tau)");
+		return;
+	}
+
+	RigidBodyDynamics::ForwardDynamics (*model, q, qdot, tau, qddot);
+
+	mwSize result_dim[2];
+	result_dim[0] = static_cast<mwSize> (qddot.size());
+	result_dim[1];
+
+	plhs[0] = (mxArray*) mxCreateNumericArray (
+			1,
+			result_dim,
+			mxGetClassID (prhs[1]),
+			mxREAL);
+
+	double *value_ptr = mxGetPr (plhs[0]);
+
+	for (unsigned int i = 0; i < qddot.size(); i++) {
+		value_ptr[i] = qddot[i];
+	}
+}

File wrapper/rbdlGetGravity.cc

+#include "mex.h"
+#include "rbdl_mex.h"
+#include "ObjectHandle.h"
+
+using namespace RigidBodyDynamics;
+
+void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
+	if (nrhs != 1) {
+		mexErrMsgTxt ("usage: gravity = rbdlGetGravity(model)");
+	}
+
+	ObjectHandle<Model> *handle = ObjectHandle<Model>::from_mex_handle(prhs[0]);
+	Model* model = &(handle->get_object());
+
+	mwSize result_dim[2];
+	result_dim[0] = static_cast<mwSize> (3);
+	result_dim[1];
+
+	plhs[0] = (mxArray*) mxCreateNumericArray (
+			1,
+			result_dim,
+			mxDOUBLE_CLASS,
+			mxREAL);
+
+	double *value_ptr = mxGetPr (plhs[0]);
+
+	for (unsigned int i = 0; i < 3; i++) {
+		value_ptr[i] = model->gravity[i];
+	}
+}

File wrapper/rbdlInverseDynamics.cc

+#include "mex.h"
+#include "rbdl_mex.h"
+#include "ObjectHandle.h"
+
+#include <string>
+
+using namespace RigidBodyDynamics;
+
+void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
+	if (nrhs != 4) {
+		mexErrMsgTxt ("usage: tau = rbdlInverseDynamics(model, q, qdot, qddot");
+		return;
+	}
+
+	ObjectHandle<Model> *handle = ObjectHandle<Model>::from_mex_handle(prhs[0]);
+	Model *model = &(handle->get_object());
+
+	if (!mxIsNumeric(prhs[1])) {
+		mexErrMsgTxt("Argument 2 has to be the parent id (i.e. a number!");
+		return;
+	}
+
+	RigidBodyDynamics::Math::VectorNd q (model->dof_count), 
+					 qdot(model->dof_count), 
+					 qddot (model->dof_count),
+					 tau (model->dof_count);
+
+	if (!GetVectorNd (prhs[1], q)) {
+		mexErrMsgTxt ("Error parsing argument 1 (q)");
+		return;
+	}
+
+	if (!GetVectorNd (prhs[2], qdot)) {
+		mexErrMsgTxt ("Error parsing argument 2 (qdot)");
+		return;
+	}
+
+	if (!GetVectorNd (prhs[3], qddot)) {
+		mexErrMsgTxt ("Error parsing argument 3 (qddot)");
+		return;
+	}
+
+	RigidBodyDynamics::InverseDynamics (*model, q, qdot, qddot, tau);
+
+	mwSize result_dim[2];
+	result_dim[0] = static_cast<mwSize> (tau.size());
+	result_dim[1];
+
+	plhs[0] = (mxArray*) mxCreateNumericArray (
+			1,
+			result_dim,
+			mxGetClassID (prhs[1]),
+			mxREAL);
+
+	double *value_ptr = mxGetPr (plhs[0]);
+
+	for (unsigned int i = 0; i < tau.size(); i++) {
+		value_ptr[i] = tau[i];
+	}
+}

File wrapper/rbdlSetGravity.cc

+#include "mex.h"
+#include "rbdl_mex.h"
+#include "ObjectHandle.h"
+
+using namespace RigidBodyDynamics;
+
+void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
+	if (nrhs != 2) {
+		mexErrMsgTxt ("usage: rbdlSetGravity(model, gravity)");
+	}
+
+	ObjectHandle<Model> *handle = ObjectHandle<Model>::from_mex_handle(prhs[0]);
+	Model* model = &(handle->get_object());
+
+	Math::Vector3d gravity;
+	if (!GetVector3d (prhs[1], gravity)) {
+		mexErrMsgTxt ("Error parsing gravity argument (expected 3 x 1 vector)");
+		return;
+	}
+
+	model->gravity = gravity;
+}

File wrapper/rbdl_mex.h

 #include "rbdl.h"
 
-bool GetVector3d (mxArray *array, RigidBodyDynamics::Math::Vector3d &vector3d_out) {
+bool GetVectorNd (const mxArray *array, RigidBodyDynamics::Math::VectorNd &vector_out) {
+	if (!mxIsDouble(array)) {
+		mexErrMsgTxt("Error: expected an array of double values!");
+		return false;
+	}
+
+	mwSize *dimensions = mxGetDimensions (array);
+
+	if (dimensions[1] != 1) {
+		mexErrMsgTxt ("Error: expected a column vector for variable size vector!");
+		return false;
+	}
+
+	unsigned int dim = static_cast<unsigned int> (dimensions[0] * dimensions[1]);
+
+	if (vector_out.size() != dim)
+		vector_out = RigidBodyDynamics::Math::VectorNd(dim);
+
+	double *value_ptr = mxGetPr (array);
+
+	for (unsigned int i = 0; i < dim; i++)
+		vector_out[i] = value_ptr[i];
+
+	return true;
+}
+
+bool GetVector3d (const mxArray *array, RigidBodyDynamics::Math::Vector3d &vector3d_out) {
 	mwSize *dimensions = mxGetDimensions (array);
 
 	if (!mxIsDouble(array)) {
 	return true;
 }
 
-bool GetMatrix3d (mxArray *array, RigidBodyDynamics::Math::Matrix3d &matrix3d_out) {
+bool GetMatrix3d (const mxArray *array, RigidBodyDynamics::Math::Matrix3d &matrix3d_out) {
 	mwSize *dimensions = mxGetDimensions (array);
 
 	if (!mxIsDouble(array)) {