rbdl / src / Joint.cc

The dev branch has multiple heads

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62``` ```/* * RBDL - Rigid Body Dynamics Library * Copyright (c) 2011-2012 Martin Felis * * Licensed under the zlib license. See LICENSE for more details. */ #include #include #include #include "rbdl/Logging.h" #include "rbdl/Model.h" #include "rbdl/Joint.h" namespace RigidBodyDynamics { using namespace Math; void jcalc ( const Model &model, const unsigned int &joint_id, SpatialTransform &XJ, SpatialVector &S, SpatialVector &v_J, SpatialVector &c_J, const double &q, const double &qdot ) { // exception if we calculate it for the root body assert (joint_id > 0); // Set the joint axis S = model.mJoints[joint_id].mJointAxes[0]; // the velocity dependent spatial acceleration is != 0 only for rhenomic // constraints (see RBDA, p. 55) c_J.setZero(); if (model.mJoints[joint_id].mJointType == JointTypeRevolute) { XJ = Xrot (q, Vector3d ( model.mJoints[joint_id].mJointAxes[0][0], model.mJoints[joint_id].mJointAxes[0][1], model.mJoints[joint_id].mJointAxes[0][2] )); } else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) { XJ = Xtrans ( Vector3d ( model.mJoints[joint_id].mJointAxes[0][3] * q, model.mJoints[joint_id].mJointAxes[0][4] * q, model.mJoints[joint_id].mJointAxes[0][5] * q ) ); } else { // Only revolute joints supported so far assert (0); } v_J = S * qdot; } } ```