diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 2cfa3a5285..4ac2a3f7d0 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -122,6 +122,7 @@ #include "SimbodyEngine/CustomJoint.h" #include "SimbodyEngine/WeldJoint.h" #include "SimbodyEngine/ScapulothoracicJoint.h" +#include "OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.h" #include "SimbodyEngine/TransformAxis.h" #include "SimbodyEngine/Coordinate.h" #include "SimbodyEngine/SpatialTransform.h" @@ -225,6 +226,7 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( PinJoint() ); Object::registerType( SliderJoint() ); Object::registerType( PlanarJoint() ); + Object::registerType( ConstantCurvatureJoint() ); Object::registerType( TransformAxis() ); Object::registerType( Coordinate() ); Object::registerType( SpatialTransform() ); diff --git a/OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.cpp b/OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.cpp new file mode 100644 index 0000000000..d456a872cc --- /dev/null +++ b/OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.cpp @@ -0,0 +1,992 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ConstantCurvatureJoint.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2023 Stanford University and the Authors * + * Author(s): Keenon Werling * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +//============================================================================== +// INCLUDES +//============================================================================== +#include "ConstantCurvatureJoint.h" + +#include "simbody/internal/MobilizedBody_Custom.h" + +#include +#include +#include +#include + +#include + +using namespace SimTK; + +// We use this definition of PI to exactly numerically match the Nimble +// implementation, so that the precomputed values for the unit tests are +// correct. +#ifndef M_PI +# define M_PI 3.14159 +#endif + +Vec3 OpenSim::ConstantCurvatureJoint::clamp(const SimTK::Vec3& q) { + Vec3 pos = q; + double bound = (M_PI / 2) - 0.01; + if (pos(0) > bound) { + log_warn(fmt::format( + "WARNING! ConstantCurvatureJoint position outside of " + "its supported range! X rotation reached position " + "{}, which is above upper bound {}. This will lead " + "to unphysical behavior. Please adjust your model " + "or simulation to avoid this state.", + pos(0), bound)); + assert(false); + pos(0) = bound; + } + if (pos(0) < -bound) { + log_warn(fmt::format( + "WARNING! ConstantCurvatureJoint position outside of " + "its supported range! X rotation reached position " + "{}, which is below lower bound {}. This will lead " + "to unphysical behavior. Please adjust your model " + "or simulation to avoid this state.", + pos(0), -bound)); + assert(false); + pos(0) = -bound; + } + if (pos(1) > bound) { + log_warn(fmt::format( + "WARNING! ConstantCurvatureJoint position outside of " + "its supported range! Z rotation reached position " + "{}, which is above upper bound {}. This will lead " + "to unphysical behavior. Please adjust your model " + "or simulation to avoid this state.", + pos(1), bound)); + assert(false); + pos(1) = bound; + } + if (pos(1) < -bound) { + log_warn(fmt::format( + "WARNING! ConstantCurvatureJoint position outside of " + "its supported range! Z rotation reached position " + "{}, which is below lower bound {}. This will lead " + "to unphysical behavior. Please adjust your model " + "or simulation to avoid this state.", + pos(1), -bound)); + assert(false); + pos(1) = -bound; + } + if (pos(2) > bound) { + log_warn(fmt::format( + "WARNING! ConstantCurvatureJoint position outside of " + "its supported range! Y rotation reached position " + "{}, which is above upper bound {}. This will lead " + "to unphysical behavior. Please adjust your model " + "or simulation to avoid this state.", + pos(2), bound)); + assert(false); + pos(2) = bound; + } + if (pos(2) < -bound) { + log_warn(fmt::format( + "WARNING! ConstantCurvatureJoint position outside of " + "its supported range! Y rotation reached position " + "{}, which is below lower bound {}. This will lead " + "to unphysical behavior. Please adjust your model " + "or simulation to avoid this state.", + pos(2), -bound)); + assert(false); + pos(2) = -bound; + } + return pos; +} + +Rotation OpenSim::ConstantCurvatureJoint::eulerXZYToMatrix(const Vec3& _angle) { + // +- -+ +- -+ + // | r00 r01 r02 | | cy*cz -sz cz*sy | + // | r10 r11 r12 | = | sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz | + // | r20 r21 r22 | | -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz | + // +- -+ +- -+ + + Mat33 ret; + + const double cx = cos(_angle(0)); + const double sx = sin(_angle(0)); + const double cz = cos(_angle(1)); + const double sz = sin(_angle(1)); + const double cy = cos(_angle(2)); + const double sy = sin(_angle(2)); + + ret(0, 0) = cy * cz; + ret(1, 0) = sx * sy + cx * cy * sz; + ret(2, 0) = -cx * sy + cy * sx * sz; + + ret(0, 1) = -sz; + ret(1, 1) = cx * cz; + ret(2, 1) = cz * sx; + + ret(0, 2) = cz * sy; + ret(1, 2) = -cy * sx + cx * sy * sz; + ret(2, 2) = cx * cy + sx * sy * sz; + + return Rotation(ret, true); +} + +/// This gives the gradient of an XZY rotation matrix with respect to the +/// specific index (0, 1, or 2) +Mat33 OpenSim::ConstantCurvatureJoint::eulerXZYToMatrixGrad( + const Vec3& _angle, int index) { + // Original + // +- -+ +- -+ + // | r00 r01 r02 | | cy*cz -sz cz*sy | + // | r10 r11 r12 | = | sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz | + // | r20 r21 r22 | | -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz | + // +- -+ +- -+ + + Mat33 ret; + + const double cx = cos(_angle(0)); + const double sx = sin(_angle(0)); + const double cz = cos(_angle(1)); + const double sz = sin(_angle(1)); + const double cy = cos(_angle(2)); + const double sy = sin(_angle(2)); + + if (index == 0) { + ret(0, 0) = 0; + ret(1, 0) = (cx)*sy + (-sx) * cy * sz; + ret(2, 0) = -(-sx) * sy + cy * (cx)*sz; + + ret(0, 1) = 0; + ret(1, 1) = (-sx) * cz; + ret(2, 1) = cz * (cx); + + ret(0, 2) = 0; + ret(1, 2) = -cy * (cx) + (-sx) * sy * sz; + ret(2, 2) = (-sx) * cy + (cx)*sy * sz; + } else if (index == 1) { + ret(0, 0) = cy * (-sz); + ret(1, 0) = cx * cy * (cz); + ret(2, 0) = cy * sx * (cz); + + ret(0, 1) = -(cz); + ret(1, 1) = cx * (-sz); + ret(2, 1) = (-sz) * sx; + + ret(0, 2) = (-sz) * sy; + ret(1, 2) = cx * sy * (cz); + ret(2, 2) = sx * sy * (cz); + } else if (index == 2) { + ret(0, 0) = (-sy) * cz; + ret(1, 0) = sx * (cy) + cx * (-sy) * sz; + ret(2, 0) = -cx * (cy) + (-sy) * sx * sz; + + ret(0, 1) = 0; + ret(1, 1) = 0; + ret(2, 1) = 0; + + ret(0, 2) = cz * (cy); + ret(1, 2) = -(-sy) * sx + cx * (cy)*sz; + ret(2, 2) = cx * (-sy) + sx * (cy)*sz; + } else { + assert(false); + } + + return ret; +} + +Mat63 OpenSim::ConstantCurvatureJoint::getEulerJacobian(const Vec3& q) { + Mat63 J; + J.setToZero(); + + // s_t q0 = _positions[0]; + const double q1 = q(1); + const double q2 = q(2); + + // s_t c0 = cos(q0); + const double c1 = cos(q1); + const double c2 = cos(q2); + + // s_t s0 = sin(q0); + const double s1 = sin(q1); + const double s2 = sin(q2); + + //------------------------------------------------------------------------ + // S = [ c1*c2, -s2, 0 + // -s1, 0, 1 + // c1*s2, c2, 0 + // 0, 0, 0 + // 0, 0, 0 + // 0, 0, 0 ]; + //------------------------------------------------------------------------ + J(0, 0) = c1 * c2; + J(0, 1) = -s2; + J(1, 0) = -s1; + J(1, 2) = 1; + J(2, 0) = c1 * s2; + J(2, 1) = c2; + + return J; +} + +Mat63 OpenSim::ConstantCurvatureJoint::getEulerJacobianDerivWrtPos( + const Vec3& q, int index) { + assert(index < 3); + + Mat63 DJ_Dq; + DJ_Dq.setToZero(); + + const double q1 = q[1]; + const double q2 = q[2]; + + // s_t c0 = cos(q0); + const double c1 = cos(q1); + const double c2 = cos(q2); + + // s_t s0 = sin(q0); + const double s1 = sin(q1); + const double s2 = sin(q2); + + //------------------------------------------------------------------------ + // S = [ c1*c2, -s2, 0 + // -s1, 0, 1 + // c1*s2, c2, 0 + // 0, 0, 0 + // 0, 0, 0 + // 0, 0, 0 ]; + //------------------------------------------------------------------------ + + if (index == 0) { + // DS/Dq0 = 0; + } else if (index == 1) { + // DS/Dq1 = [-s1*c2, 0, 0 + // -c1, 0, 0 + // -s1*s2, 0, 0 + // 0, 0, 0 + // 0, 0, 0 + // 0, 0, 0 ]; + + DJ_Dq(0, 0) = -s1 * c2; + DJ_Dq(1, 0) = -c1; + DJ_Dq(2, 0) = -s1 * s2; + } else if (index == 2) { + // DS/Dq2 = [-c1*s2, -c2, 0 + // 0, 0, 0 + // c1*c2, -s2, 0 + // 0, 0, 0 + // 0, 0, 0 + // 0, 0, 0 ]; + + DJ_Dq(0, 0) = -c1 * s2; + DJ_Dq(2, 0) = c1 * c2; + + DJ_Dq(0, 1) = -c2; + DJ_Dq(2, 1) = -s2; + } + + return DJ_Dq; +} + +Mat63 OpenSim::ConstantCurvatureJoint::getConstantCurveJacobian( + const Vec3& pos, double d) { + // 1. Do the euler rotation + Rotation rot = eulerXZYToMatrix(pos); + Mat63 J = getEulerJacobian(pos); + + // Remember, this is X,*Z*,Y + + double cx = cos(pos(0)); + double sx = sin(pos(0)); + double cz = cos(pos(1)); + double sz = sin(pos(1)); + + Vec3 linearAngle = Vec3(-sz, cx * cz, cz * sx); + Mat33 dLinearAngle; + dLinearAngle.col(0) = Vec3(0, -sx * cz, cz * cx); + dLinearAngle.col(1) = Vec3(-cz, -cx * sz, -sz * sx); + dLinearAngle.col(2) = Vec3(0, 0, 0); + + double sinTheta = sqrt( + linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2)); + + if (sinTheta < 0.001 || sinTheta > 0.999) { + // Near very vertical angles, don't worry about the bend, just + // approximate with an euler joint + + // 2. Computing translation from vertical + Vec3 translation = rot * Vec3(0, d, 0); + + J.col(0).updSubVec<3>(3) = + 0.5 * SimTK::cross(J.col(0).getSubVec<3>(0), translation); + J.col(1).updSubVec<3>(3) = + 0.5 * SimTK::cross(J.col(1).getSubVec<3>(0), translation); + J.col(2).updSubVec<3>(3) = + 0.5 * SimTK::cross(J.col(2).getSubVec<3>(0), translation); + } else { + // Compute the bend as a function of the angle from vertical + Vec3 dSinTheta; + for (int i = 0; i < 3; i++) { + dSinTheta(i) = (0.5 / sqrt(linearAngle(0) * linearAngle(0) + + linearAngle(2) * linearAngle(2))) * + (2 * linearAngle(0) * dLinearAngle(0, i) + + 2 * linearAngle(2) * dLinearAngle(2, i)); + } + + double theta = asin(sinTheta); + Vec3 dTheta = (1.0 / sqrt(1.0 - (sinTheta * sinTheta))) * dSinTheta; + + double r = (d / theta); + Vec3 dR = (-d / (theta * theta)) * dTheta; + + double horizontalDist = r - r * cos(theta); + + Vec3 dHorizontalDist = dR + r * sin(theta) * dTheta - dR * cos(theta); + Vec3 dVerticalDist = r * cos(theta) * dTheta + dR * sinTheta; + + Mat33 dTranslation; + dTranslation.row(0) = + (linearAngle(0) / sinTheta) * dHorizontalDist.transpose() + + (horizontalDist / sinTheta) * dLinearAngle.row(0) + + (horizontalDist * linearAngle(0)) * + (-1.0 / (sinTheta * sinTheta)) * dSinTheta.transpose(); + dTranslation.row(1) = dVerticalDist.transpose(); + dTranslation.row(2) = + (linearAngle(2) / sinTheta) * dHorizontalDist.transpose() + + (horizontalDist / sinTheta) * dLinearAngle.row(2) + + (horizontalDist * linearAngle(2)) * + (-1.0 / (sinTheta * sinTheta)) * dSinTheta.transpose(); + + J.col(0).updSubVec<3>(3) = rot.transpose() * dTranslation.col(0); + J.col(1).updSubVec<3>(3) = rot.transpose() * dTranslation.col(1); + J.col(2).updSubVec<3>(3) = rot.transpose() * dTranslation.col(2); + } + + return J; +} + +Mat63 OpenSim::ConstantCurvatureJoint::getConstantCurveJacobianDerivWrtPosition( + const Vec3& pos, double d, int index) { + // 1. Do the euler rotation + const Rotation rot = eulerXZYToMatrix(pos); + const Mat33 rot_dFirst = eulerXZYToMatrixGrad(pos, index); + + Mat63 J_dFirst = getEulerJacobianDerivWrtPos(pos, index); + + // Remember, this is X,*Z*,Y + + const double cx = cos(pos(0)); + const double sx = sin(pos(0)); + const double cz = cos(pos(1)); + const double sz = sin(pos(1)); + + const Vec3 linearAngle(-sz, cx * cz, cz * sx); + + Mat33 dLinearAngle; + dLinearAngle.col(0) = Vec3(0, -sx * cz, cz * cx); + dLinearAngle.col(1) = Vec3(-cz, -cx * sz, -sz * sx); + dLinearAngle.col(2).setToZero(); + + Mat33 dLinearAngle_dFirst; + dLinearAngle_dFirst.setToZero(); + + Vec3 linearAngle_dFirst = Vec3(0, 0, 0); + if (index == 0) { + linearAngle_dFirst = Vec3(0, -sx * cz, cz * cx); + dLinearAngle_dFirst.col(0) = Vec3(0, -cx * cz, cz * -sx); + dLinearAngle_dFirst.col(1) = Vec3(0, sx * sz, -sz * cx); + dLinearAngle_dFirst.col(2).setToZero(); + } else if (index == 1) { + linearAngle_dFirst = Vec3(-cz, cx * -sz, -sz * sx); + dLinearAngle_dFirst.col(0) = Vec3(0, -sx * -sz, -sz * cx); + dLinearAngle_dFirst.col(1) = Vec3(sz, -cx * cz, -cz * sx); + dLinearAngle_dFirst.col(2).setToZero(); + } + + const double sinTheta = sqrt( + linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2)); + + if (sinTheta < 0.001 || sinTheta > 0.999) { + // Near very vertical angles, don't worry about the bend, just + // approximate with an euler joint + const Mat63 J = getEulerJacobian(pos); + + // 2. Computing translation from vertical + Vec3 translation = rot * Vec3(0, d, 0); + + const Vec3 translation_dFirst = + rot * SimTK::cross(J.col(index).getSubVec<3>(0), translation); + + J_dFirst.col(0).updSubVec<3>(3) = + 0.5 * + (SimTK::cross(J_dFirst.col(0).getSubVec<3>(0), translation) + + SimTK::cross( + J.col(0).getSubVec<3>(0), translation_dFirst)); + J_dFirst.col(1).updSubVec<3>(3) = + 0.5 * + (SimTK::cross(J_dFirst.col(1).getSubVec<3>(0), translation) + + SimTK::cross( + J.col(1).getSubVec<3>(0), translation_dFirst)); + J_dFirst.col(2).updSubVec<3>(3) = + 0.5 * + (SimTK::cross(J_dFirst.col(2).getSubVec<3>(0), translation) + + SimTK::cross( + J.col(2).getSubVec<3>(0), translation_dFirst)); + } else { + Vec3 dSinTheta; + Vec3 dSinTheta_dFirst; + for (int i = 0; i < 3; i++) { + const double part1 = + (0.5 / sqrt(linearAngle(0) * linearAngle(0) + + linearAngle(2) * linearAngle(2))); + const double part2 = (2 * linearAngle(0) * dLinearAngle(0, i) + + 2 * linearAngle(2) * dLinearAngle(2, i)); + dSinTheta(i) = part1 * part2; + + const double part1_dFirst = + ((-0.25 / pow(linearAngle(0) * linearAngle(0) + + linearAngle(2) * linearAngle(2), + 1.5)) * + (2 * linearAngle(0) * linearAngle_dFirst(0) + + 2 * linearAngle(2) * + linearAngle_dFirst(2))); + const double part2_dFirst = + (2 * linearAngle(0) * dLinearAngle_dFirst(0, i) + + 2 * linearAngle_dFirst(0) * dLinearAngle(0, i) + + 2 * linearAngle(2) * dLinearAngle_dFirst(2, i) + + 2 * linearAngle_dFirst(2) * dLinearAngle(2, i)); + + dSinTheta_dFirst(i) = part1_dFirst * part2 + part1 * part2_dFirst; + } + + const double sinTheta_dFirst = + (0.5 / sqrt(linearAngle(0) * linearAngle(0) + + linearAngle(2) * linearAngle(2))) * + (2 * linearAngle(0) * linearAngle_dFirst(0) + + 2 * linearAngle(2) * linearAngle_dFirst(2)); + + // Compute the bend as a function of the angle from vertical + const double theta = asin(sinTheta); + const double theta_dFirst = + (1.0 / sqrt(1.0 - (sinTheta * sinTheta))) * sinTheta_dFirst; + (void)theta_dFirst; + + const Vec3 dTheta = + (1.0 / sqrt(1.0 - (sinTheta * sinTheta))) * dSinTheta; + const Vec3 dTheta_dFirst = + (1.0 / pow(1.0 - (sinTheta * sinTheta), 1.5)) * sinTheta * + sinTheta_dFirst * dSinTheta + + (1.0 / sqrt(1.0 - (sinTheta * sinTheta))) * dSinTheta_dFirst; + (void)dTheta_dFirst; + + const double r = (d / theta); + const double r_dFirst = (-d / (theta * theta)) * theta_dFirst; + (void)r_dFirst; + + const Vec3 dR = (-d / (theta * theta)) * dTheta; + const Vec3 dR_dFirst = + (2 * d / (theta * theta * theta)) * theta_dFirst * dTheta + + (-d / (theta * theta)) * dTheta_dFirst; + (void)dR_dFirst; + + const double horizontalDist = r - r * cos(theta); + const double horizontalDist_dFirst = + r_dFirst - + (r_dFirst * cos(theta) - r * sin(theta) * theta_dFirst); + (void)horizontalDist_dFirst; + + const Vec3 dHorizontalDist = + dR + r * sin(theta) * dTheta - dR * cos(theta); + const Vec3 dHorizontalDist_dFirst = + dR_dFirst + + (r_dFirst * sin(theta) * dTheta + + r * cos(theta) * theta_dFirst * dTheta + + r * sin(theta) * dTheta_dFirst) - + (dR_dFirst * cos(theta) - dR * sin(theta) * theta_dFirst); + (void)dHorizontalDist_dFirst; + + const Vec3 dVerticalDist = r * cos(theta) * dTheta + dR * sinTheta; + const Vec3 dVerticalDist_dFirst = + (r_dFirst * cos(theta) * dTheta - + r * sin(theta) * theta_dFirst * dTheta + + r * cos(theta) * dTheta_dFirst) + + (dR_dFirst * sinTheta + dR * sinTheta_dFirst); + (void)dVerticalDist_dFirst; + + Mat33 dTranslation; + dTranslation.row(0) = + (linearAngle(0) / sinTheta) * dHorizontalDist.transpose() + + (horizontalDist / sinTheta) * dLinearAngle.row(0) + + (horizontalDist * linearAngle(0)) * + (-1.0 / (sinTheta * sinTheta)) * dSinTheta.transpose(); + dTranslation.row(1) = dVerticalDist.transpose(); + dTranslation.row(2) = + (linearAngle(2) / sinTheta) * dHorizontalDist.transpose() + + (horizontalDist / sinTheta) * dLinearAngle.row(2) + + (horizontalDist * linearAngle(2)) * + (-1.0 / (sinTheta * sinTheta)) * dSinTheta.transpose(); + + Mat33 dTranslation_dFirst; + dTranslation_dFirst.row(0) = + ((linearAngle_dFirst(0) / sinTheta) * + dHorizontalDist.transpose() - + (linearAngle(0) / (sinTheta * sinTheta)) * + sinTheta_dFirst * dHorizontalDist.transpose() + + (linearAngle(0) / sinTheta) * + dHorizontalDist_dFirst.transpose()) + + ((horizontalDist_dFirst / sinTheta) * dLinearAngle.row(0) - + (horizontalDist / (sinTheta * sinTheta)) * + sinTheta_dFirst * dLinearAngle.row(0) + + (horizontalDist / sinTheta) * + dLinearAngle_dFirst.row(0)) + + ((horizontalDist_dFirst * linearAngle(0) + + horizontalDist * linearAngle_dFirst(0)) * + (-1.0 / (sinTheta * sinTheta)) * + dSinTheta.transpose() + + (horizontalDist * linearAngle(0)) * + (2.0 * sinTheta_dFirst / + (sinTheta * sinTheta * sinTheta)) * + dSinTheta.transpose() + + (horizontalDist * linearAngle(0)) * + (-1.0 / (sinTheta * sinTheta)) * + dSinTheta_dFirst.transpose()); + dTranslation_dFirst.row(1) = dVerticalDist_dFirst.transpose(); + // This looks like a whole new mess, but it's actually identical to + // row(0), except with the indices changed to 2 + dTranslation_dFirst.row(2) = + ((linearAngle_dFirst(2) / sinTheta) * + dHorizontalDist.transpose() - + (linearAngle(2) / (sinTheta * sinTheta)) * + sinTheta_dFirst * dHorizontalDist.transpose() + + (linearAngle(2) / sinTheta) * + dHorizontalDist_dFirst.transpose()) + + ((horizontalDist_dFirst / sinTheta) * dLinearAngle.row(2) - + (horizontalDist / (sinTheta * sinTheta)) * + sinTheta_dFirst * dLinearAngle.row(2) + + (horizontalDist / sinTheta) * + dLinearAngle_dFirst.row(2)) + + ((horizontalDist_dFirst * linearAngle(2) + + horizontalDist * linearAngle_dFirst(2)) * + (-1.0 / (sinTheta * sinTheta)) * + dSinTheta.transpose() + + (horizontalDist * linearAngle(2)) * + (2.0 * sinTheta_dFirst / + (sinTheta * sinTheta * sinTheta)) * + dSinTheta.transpose() + + (horizontalDist * linearAngle(2)) * + (-1.0 / (sinTheta * sinTheta)) * + dSinTheta_dFirst.transpose()); + + J_dFirst.col(0).updSubVec<3>(3) = + rot.transpose() * dTranslation_dFirst.col(0) + + rot_dFirst.transpose() * dTranslation.col(0); + J_dFirst.col(1).updSubVec<3>(3) = + rot.transpose() * dTranslation_dFirst.col(1) + + rot_dFirst.transpose() * dTranslation.col(1); + J_dFirst.col(2).updSubVec<3>(3) = + rot.transpose() * dTranslation_dFirst.col(2) + + rot_dFirst.transpose() * dTranslation.col(2); + } + + return J_dFirst; +} + +Mat63 OpenSim::ConstantCurvatureJoint::getConstantCurveJacobianDerivWrtTime( + const Vec3& pos, const Vec3& dPos, double d) { + Mat63 dJ; + dJ.setToZero(); + for (int i = 0; i < 3; i++) { + dJ += dPos(i) * getConstantCurveJacobianDerivWrtPosition(pos, d, i); + } + return dJ; +} + +Transform OpenSim::ConstantCurvatureJoint::getTransform(Vec3 pos, double d) { + // 1. Do the euler rotation + Rotation rot = eulerXZYToMatrix(pos); + + // 2. Computing translation from vertical + double cx = cos(pos(0)); + double sx = sin(pos(0)); + double cz = cos(pos(1)); + double sz = sin(pos(1)); + + Vec3 linearAngle = Vec3(-sz, cx * cz, cz * sx); // rot.linear().col(1); + + double sinTheta = sqrt( + linearAngle(0) * linearAngle(0) + linearAngle(2) * linearAngle(2)); + Vec3 translation(0, d, 0); + if (sinTheta < 0.001 || sinTheta > 0.999) { + // Near very vertical angles, don't worry about the bend, just + // approximate with an euler joint + translation = rot * translation; + } else { + // Compute the bend as a function of the angle from vertical + double theta = asin(sinTheta); + double r = (d / theta); + double horizontalDist = r - r * cos(theta); + double verticalDist = r * sinTheta; + + translation = Vec3(horizontalDist * (linearAngle(0) / sinTheta), + verticalDist, horizontalDist * (linearAngle(2) / sinTheta)); + } + + return Transform(rot, translation); +} + +//============================================================================== +// IMPLEMENTATION +//============================================================================== +class ConstantCurvatureJointImpl + : public MobilizedBody::Custom::Implementation { +public: + ConstantCurvatureJointImpl(SimbodyMatterSubsystem& matter) + : MobilizedBody::Custom::Implementation(matter, 3, 3, 0), + neutralPos(0, 0, 0), length(1.0) {} + + MobilizedBody::Custom::Implementation* clone() const { + return new ConstantCurvatureJointImpl(*this); + }; + + Transform calcMobilizerTransformFromQ( + const State& s, int nq, const Real* q) const { + return OpenSim::ConstantCurvatureJoint::getTransform( + OpenSim::ConstantCurvatureJoint::clamp( + Vec3::getAs(q) + neutralPos), + length); + } + + SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const { + auto rawQ = getQ(s); + Vec3 q = Vec3(rawQ(0), rawQ(1), rawQ(2)); + Mat63 J = OpenSim::ConstantCurvatureJoint::getConstantCurveJacobian( + OpenSim::ConstantCurvatureJoint::clamp(q + neutralPos), length); + Vec6 result = J * Vec3(u[0], u[1], u[2]); + + assert(!q.isNaN()); + assert(!J.isNaN()); + assert(!J.isInf()); + assert(!result.isNaN()); + + return SpatialVec(result.getSubVec<3>(0), result.getSubVec<3>(3)); + } + + void multiplyByHTranspose( + const State& s, const SpatialVec& F, int nu, Real* f) const { + auto rawQ = getQ(s); + Vec3 q = Vec3(rawQ(0), rawQ(1), rawQ(2)); + Mat63 J = OpenSim::ConstantCurvatureJoint::getConstantCurveJacobian( + OpenSim::ConstantCurvatureJoint::clamp(q + neutralPos), length); + + Vec6 rawSpatial; + rawSpatial.updSubVec<3>(0) = F[0]; + rawSpatial.updSubVec<3>(3) = F[1]; + + Vec3 result = J.transpose() * rawSpatial; + + assert(!q.isNaN()); + assert(!J.isNaN()); + assert(!J.isInf()); + assert(!result.isNaN()); + + Vec3::updAs(f) = result; + } + + + SpatialVec multiplyByHDotMatrix( + const State& s, int nu, const Real* u) const { + // TODO: Revisit this method is necessary. + return SpatialVec(Vec3(0), Vec3(0)); + auto rawQ = getQ(s); + auto rawQDot = getQDot(s); + Vec3 q = Vec3(rawQ(0), rawQ(1), rawQ(2)); + Vec3 dq = Vec3(rawQDot(0), rawQDot(1), rawQDot(2)); + Mat63 dJ = OpenSim::ConstantCurvatureJoint:: + getConstantCurveJacobianDerivWrtTime( + OpenSim::ConstantCurvatureJoint::clamp(q + neutralPos), + dq, length); + Vec6 result = dJ * Vec3(u[0], u[1], u[2]); + + assert(!q.isNaN()); + assert(!dq.isNaN()); + assert(!dJ.isNaN()); + assert(!dJ.isInf()); + assert(!result.isNaN()); + + return SpatialVec(result.getSubVec<3>(0), result.getSubVec<3>(3)); + } + + void multiplyByHDotTranspose( + const State& s, const SpatialVec& F, int nu, Real* f) const { + auto rawQ = getQ(s); + auto rawQDot = getQDot(s); + Vec3 q = Vec3(rawQ(0), rawQ(1), rawQ(2)); + Vec3 dq = Vec3(rawQDot(0), rawQDot(1), rawQDot(2)); + Mat63 dJ = OpenSim::ConstantCurvatureJoint:: + getConstantCurveJacobianDerivWrtTime( + OpenSim::ConstantCurvatureJoint::clamp(q + neutralPos), + dq, length); + + Vec6 rawSpatial; + rawSpatial.updSubVec<3>(0) = F[0]; + rawSpatial.updSubVec<3>(3) = F[1]; + + Vec3 result = dJ.transpose() * rawSpatial; + + assert(!q.isNaN()); + assert(!dq.isNaN()); + assert(!dJ.isNaN()); + assert(!dJ.isInf()); + assert(!result.isNaN()); + + Vec3::updAs(f) = result; + } + + void setQToFitTransform( + const State&, const Transform& X_FM, int nq, Real* q) const { + assert(false); + Vec3::updAs(q) = X_FM.p(); + } + + void setUToFitVelocity( + const State&, const SpatialVec& V_FM, int nu, Real* u) const { + assert(false); + Vec3::updAs(u) = V_FM[1]; + } + + void setLength(double l) { length = l; } + + void setNeutralPos(Vec3 neutral) { neutralPos = neutral; } + +protected: + Vec3 neutralPos; + double length; +}; + +class ConstantCurvatureJointWrapper : public MobilizedBody::Custom { +public: + ConstantCurvatureJointWrapper(MobilizedBody& parent, const Transform& X_PF, + const SimTK::Body& bodyInfo, const Transform& X_BM, + Direction direction = Forward) + : MobilizedBody::Custom(parent, + new ConstantCurvatureJointImpl( + parent.updMatterSubsystem()), + X_PF, bodyInfo, X_BM, direction){}; + + ConstantCurvatureJointWrapper(MobilizedBody& parent, + const SimTK::Body& bodyInfo, Direction direction = Forward) + : MobilizedBody::Custom(parent, + new ConstantCurvatureJointImpl( + parent.updMatterSubsystem()), + bodyInfo, direction){}; + + void setLength(double l) { + static_cast(updImplementation()) + .setLength(l); + } + + void setNeutralPos(Vec3 neutral) { + static_cast(updImplementation()) + .setNeutralPos(neutral); + } + + // Returns (Vec4,Vec3) where the Vec4 is a normalized quaternion. + const Vec3 getDefaultQ() const { return Vec3(Zero); } +}; + +//============================================================================== +// STATICS +//============================================================================== +using namespace std; +using namespace SimTK; +using namespace OpenSim; + +//============================================================================== +// CONSTRUCTORS AND DESTRUCTOR +//============================================================================== +ConstantCurvatureJoint::ConstantCurvatureJoint() : Super() { + constructProperties(); +} + +ConstantCurvatureJoint::ConstantCurvatureJoint(const std::string& name, + const PhysicalFrame& parent, const PhysicalFrame& child, + const SimTK::Vec3& neutralAngleXZY, const double length) + : Super(name, parent, child) { + constructProperties(); + set_neutral_angle_x_z_y(neutralAngleXZY); + set_length(length); +} + +ConstantCurvatureJoint::ConstantCurvatureJoint(const std::string& name, + const PhysicalFrame& parent, const SimTK::Vec3& locationInParent, + const SimTK::Vec3& orientationInParent, const PhysicalFrame& child, + const SimTK::Vec3& locationInChild, + const SimTK::Vec3& orientationInChild, + const SimTK::Vec3& neutralAngleXZY, const double length) + : Super(name, parent, locationInParent, orientationInParent, child, + locationInChild, orientationInChild) { + constructProperties(); + set_neutral_angle_x_z_y(neutralAngleXZY); + set_length(length); +} + +//============================================================================= +// CONSTRUCTION +//============================================================================= + +//_____________________________________________________________________________ +/** + * Connect properties to local pointers. + */ +void ConstantCurvatureJoint::constructProperties() { + setAuthors("Keenon Werling"); + SimTK::Vec3 neutralAngleXZY(Zero); + constructProperty_neutral_angle_x_z_y(neutralAngleXZY); + constructProperty_length(1.0); +} + +//============================================================================= +// GET AND SET +//============================================================================= +//_____________________________________________________________________________ +/** + * Set the ConstantCurvatureJoint's neutral angle, which it will bend to when + * its DOFs are set to zero. + * + * @param Vec3 of radii: X, Y, Z in the parent frame. + */ +void ConstantCurvatureJoint::setNeutralAngleXZY( + const SimTK::Vec3& neutralAngleXZY) { + set_neutral_angle_x_z_y(neutralAngleXZY); +} + +void ConstantCurvatureJoint::setLength(const double length) { + set_length(length); +} + +//============================================================================== +// SCALING +//============================================================================== +void ConstantCurvatureJoint::extendScale( + const SimTK::State& s, const ScaleSet& scaleSet) { + Super::extendScale(s, scaleSet); + + // Get scale factors (if an entry for the parent Frame's base Body exists). + const Vec3& scaleFactors = getScaleFactors(scaleSet, getChildFrame()); + if (scaleFactors == ModelComponent::InvalidScaleFactors) return; + + // TODO: Need to scale transforms appropriately, given an arbitrary axis. + upd_length() = get_length() * scaleFactors.get(1); +} + +//============================================================================= +// Simbody Model building. +//============================================================================= +//_____________________________________________________________________________ +void ConstantCurvatureJoint::extendAddToSystem( + SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + // CREATE MOBILIZED BODY + ConstantCurvatureJointWrapper mobod = + createMobilizedBody(system); + mobod.setLength(get_length()); + mobod.setNeutralPos(get_neutral_angle_x_z_y()); +} + +void ConstantCurvatureJoint::extendInitStateFromProperties( + SimTK::State& s) const { + Super::extendInitStateFromProperties(s); + + getCoordinate(ConstantCurvatureJoint::Coord::RotationX).setClamped(s, true); + getCoordinate(ConstantCurvatureJoint::Coord::RotationY).setClamped(s, true); + getCoordinate(ConstantCurvatureJoint::Coord::RotationZ).setClamped(s, true); + + /* + double xangle = getCoordinate(ConstantCurvatureJoint::Coord::Rotation1X) + .getDefaultValue(); + double yangle = getCoordinate(ConstantCurvatureJoint::Coord::Rotation2Y) + .getDefaultValue(); + double zangle = getCoordinate(ConstantCurvatureJoint::Coord::Rotation3Z) + .getDefaultValue(); + SimTK::Vector v(3); + v.set(0, xangle); + v.set(1, yangle); + v.set(2, zangle); + + getChildFrame().getMobilizedBody().setQFromVector(s, v); + */ +} + +void ConstantCurvatureJoint::extendSetPropertiesFromState( + const SimTK::State& state) { + Super::extendSetPropertiesFromState(state); + + /* + // Override default in case of quaternions. + const SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem(); + if (!matter.getUseEulerAngles(state)) { + + Rotation r = getChildFrame().getMobilizedBody().getBodyRotation(state); + Vec3 angles = r.convertRotationToBodyFixedXYZ(); + + updCoordinate(ConstantCurvatureJoint::Coord::Rotation1X) + .setDefaultValue(angles[0]); + updCoordinate(ConstantCurvatureJoint::Coord::Rotation2Y) + .setDefaultValue(angles[1]); + updCoordinate(ConstantCurvatureJoint::Coord::Rotation3Z) + .setDefaultValue(angles[2]); + } + */ +} + +void ConstantCurvatureJoint::generateDecorations(bool fixed, + const ModelDisplayHints& hints, const SimTK::State& state, + SimTK::Array_& geometryArray) const { + // invoke parent class method, this draws 2 Frames + Super::generateDecorations(fixed, hints, state, geometryArray); + // The next line is necessary to avoid ellipsoid below added twice + // since this method is called with fixed={true, false} + if (fixed) return; + + // Construct the visible line + double length = get_length(); + Vec3 neutralPos = get_neutral_angle_x_z_y(); + auto rawQ = getChildFrame().getMobilizedBody().getQAsVector(state); + Vec3 q = Vec3(rawQ(0), rawQ(1), rawQ(2)); + Vec3 pos = clamp(neutralPos + q); + + std::vector points; + const int numPoints = 10; + for (int i = 0; i <= numPoints; i++) { + double percentage = ((double)i) / numPoints; + + Transform t = + getTransform(clamp(pos * percentage), length * percentage); + points.push_back(t.p()); + } + + for (int i = 1; i < (int)points.size(); i++) { + Vec3 lastPoint = points[i - 1]; + Vec3 thisPoint = points[i]; + SimTK::DecorativeLine line(lastPoint, thisPoint); + const OpenSim::PhysicalFrame& frame = getParentFrame(); + line.setColor(Vec3(1.0, 0.0, 0.0)); + line.setBodyId(frame.getMobilizedBodyIndex()); + line.setTransform(frame.findTransformInBaseFrame()); + geometryArray.push_back(line); + } +} diff --git a/OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.h b/OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.h new file mode 100644 index 0000000000..105c31e781 --- /dev/null +++ b/OpenSim/Simulation/SimbodyEngine/ConstantCurvatureJoint.h @@ -0,0 +1,241 @@ +#ifndef OPENSIM_CONSTANT_CURVATURE_JOINT_H_ +#define OPENSIM_CONSTANT_CURVATURE_JOINT_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: ConstantCurveJoint.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2023 Stanford University and the Authors * + * Author(s): Keenon Werling * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +// INCLUDE +#include "Joint.h" + +namespace OpenSim { + +//============================================================================== +//============================================================================== +/** + +A class implementing a ConstantCurvatureJoint joint. A ConstantCurvatureJoint +connects two bodies by a line segment of a fixed length. The endpoint of the +ConstantCurvatureJoint can be rotated by euler angles, and the offset is +computed as a function of the euler angles and the fixed length of the line +segment. + +This joint was originally designed as a lightweight way to model spine segments, +which can be approximated without individual link segments be instead using 3 of +these joints in series. + +@author Keenon Werling +*/ +class OSIMSIMULATION_API ConstantCurvatureJoint : public Joint { + OpenSim_DECLARE_CONCRETE_OBJECT(ConstantCurvatureJoint, Joint); + +public: + /** Indices of Coordinates for use as arguments to getCoordinate() and + updCoordinate(). + + C++ example + \code{.cpp} + const auto& rx = myConstantCurvatureJoint.getCoordinate( + ConstantCurvatureJoint::Coord::Rotation1X); + \endcode + + Python example + \code{.py} + import opensim + rx = myConstantCurvatureJoint.getCoordinate( + opensim.ConstantCurvatureJoint.Coord_Rotation1X) + \endcode + + Java example + \code{.java} + rx = myConstantCurvatureJoint.getCoordinate( + ConstantCurvatureJoint.Coord.Rotation1X); + \endcode + + MATLAB example + \code{.m} + rx = myConstantCurvatureJoint.get_coordinates(0); + \endcode + + @note + The joint has an **X-Z-Y** rotation ordering. We use this ordering + because that means the first two DOFs are rotation and translation, + while the last DOF is merely a "twist" of the segment without any + translation. This is also for compatibility with the original + implementation in Nimble Physics. + */ + enum class Coord : unsigned { + RotationX = 0u, ///< 0 + RotationZ = 1u, ///< 1 + RotationY = 2u, ///< 2 + }; + +private: + /** Specify the Coordinates of the ConstantCurvatureJoint. */ + CoordinateIndex rx{constructCoordinate(Coordinate::MotionType::Rotational, + static_cast(Coord::RotationX))}; + CoordinateIndex rz{constructCoordinate(Coordinate::MotionType::Rotational, + static_cast(Coord::RotationZ))}; + CoordinateIndex ry{constructCoordinate(Coordinate::MotionType::Rotational, + static_cast(Coord::RotationY))}; + +public: + //========================================================================== + // PROPERTIES + //========================================================================== + OpenSim_DECLARE_PROPERTY(neutral_angle_x_z_y, SimTK::Vec3, + "The neutral angle of the endpoint as a Vec3(rX, rY, rZ)."); + + OpenSim_DECLARE_PROPERTY(length, double, "Length of line segment."); + + //========================================================================== + // METHODS + //========================================================================== + // CONSTRUCTION + ConstantCurvatureJoint(); + /** Convenience Joint like Constructor */ + ConstantCurvatureJoint(const std::string& name, const PhysicalFrame& parent, + const PhysicalFrame& child, const SimTK::Vec3& neutralAngleXZY, + const double length); + + /** Deprecated Joint Constructor + NOTE(keenon): This constructor seems necessary to compile, but it has a + comment marking it deprecated in the EllipsoidJoint, so I copied that + comment over here as well. */ + ConstantCurvatureJoint(const std::string& name, const PhysicalFrame& parent, + const SimTK::Vec3& locationInParent, + const SimTK::Vec3& orientationInParent, const PhysicalFrame& child, + const SimTK::Vec3& locationInChild, + const SimTK::Vec3& orientationInChild, + const SimTK::Vec3& neutralAngleXZY, const double length); + + // Set properties + void setNeutralAngleXZY(const SimTK::Vec3& neutralAngleXZY); + void setLength(const double length); + + /** Exposes getCoordinate() method defined in base class (overloaded below). + @see Joint */ + using Joint::getCoordinate; + + /** Exposes updCoordinate() method defined in base class (overloaded below). + @see Joint */ + using Joint::updCoordinate; + + /** Get a const reference to a Coordinate associated with this Joint. + @see Coord */ + const Coordinate& getCoordinate(Coord idx) const { + return get_coordinates(static_cast(idx)); + } + + /** Get a writable reference to a Coordinate associated with this Joint. + @see Coord */ + Coordinate& updCoordinate(Coord idx) { + return upd_coordinates(static_cast(idx)); + } + + // SCALE + void extendScale(const SimTK::State& s, const ScaleSet& scaleSet) override; + + //========================================================================== + // Public, static math utility functions. + //========================================================================== + + /** This method will clamp an input set of joint angles q to the limits of + * the joint, and return the clamped vector. + */ + static SimTK::Vec3 clamp(const SimTK::Vec3& q); + + /** This method will convert a vector of X,Z,Y rotations into the + * corresponding SO3 rotation matrix. + */ + static SimTK::Rotation eulerXZYToMatrix(const SimTK::Vec3& _angle); + + /** This method will convert an SO3 rotation matrix into a corresponding + * vector of X,Z,Y rotations. + */ + static SimTK::Mat33 eulerXZYToMatrixGrad( + const SimTK::Vec3& _angle, int index); + + /** This method will return the Jacobian of a pure Euler joint (following + * the XZY convention), where each column gives the derivative of the + * spatial (SE3) coordinates for the joint transform wrt one degree of + * freedom of the joint (so there are 3, and each is of dimension 6). + */ + static SimTK::Mat63 getEulerJacobian(const SimTK::Vec3& q); + + /** This method will return the derivative of the matrix returned by + * getEulerJacobian, with respect to changes to the `index` DOF of the + * joint. This is the same shape as the original matrix, because we take + * the derivative of every entry of the matrix separately. + */ + static SimTK::Mat63 getEulerJacobianDerivWrtPos( + const SimTK::Vec3& q, int index); + + /** This is much like getEulerJacobian(), because the rotational component + * is exactly the same, but the translational component is now non-zero. + * This takes as input the length of the line segment, `d`. + */ + static SimTK::Mat63 getConstantCurveJacobian( + const SimTK::Vec3& pos, double d); + + /** This method will return the derivative of the matrix returned by + * getConstantCurveJacobian, with respect to changes to the `index` DOF of + * the joint. This is the same shape as the original matrix, because we + * take the derivative of every entry of the matrix separately. + */ + static SimTK::Mat63 getConstantCurveJacobianDerivWrtPosition( + const SimTK::Vec3& pos, double d, int index); + + /** This method will return the derivative of the matrix returned by + * getConstantCurveJacobian, with respect to time (changes to every element + * in `pos` at rate `dPos`). This is the same shape as the original matrix, + * because we take the derivative of every entry of the matrix separately. + */ + static SimTK::Mat63 getConstantCurveJacobianDerivWrtTime( + const SimTK::Vec3& pos, const SimTK::Vec3& dPos, double d); + + /** This computes a transform for a given DOF position (XZY euler rotation) + * and line segment length. */ + static SimTK::Transform getTransform(SimTK::Vec3 pos, double d); + +protected: + // ModelComponent interface. + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendInitStateFromProperties(SimTK::State& s) const override; + void extendSetPropertiesFromState(const SimTK::State& state) override; + + // Visual support in SimTK visualizer + void generateDecorations(bool fixed, const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& geometryArray) + const override; + +private: + void constructProperties(); + +//============================================================================== +}; // END of class ConstantCurvatureJoint +//============================================================================== +//============================================================================== + +} // end of namespace OpenSim + +#endif // OPENSIM_CONSTANT_CURVATURE_JOINT_H_ diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 173bd34a95..dc85551b82 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -112,6 +112,7 @@ #include "SimbodyEngine/TransformAxis.h" #include "SimbodyEngine/Coordinate.h" #include "SimbodyEngine/SpatialTransform.h" +#include "SimbodyEngine/ConstantCurvatureJoint.h" #include "AssemblySolver.h" #include "CoordinateReference.h" diff --git a/OpenSim/Tests/CMakeLists.txt b/OpenSim/Tests/CMakeLists.txt index 84eb86837e..38e536d29c 100644 --- a/OpenSim/Tests/CMakeLists.txt +++ b/OpenSim/Tests/CMakeLists.txt @@ -16,5 +16,6 @@ if(BUILD_TESTING) add_subdirectory(AnalysisPluginExample) add_subdirectory(BodyDragExample) add_subdirectory(BuildDynamicWalker) + add_subdirectory(ConstantCurvatureExample) endif() diff --git a/OpenSim/Tests/ConstantCurvatureExample/CMakeLists.txt b/OpenSim/Tests/ConstantCurvatureExample/CMakeLists.txt new file mode 100644 index 0000000000..0c8942e101 --- /dev/null +++ b/OpenSim/Tests/ConstantCurvatureExample/CMakeLists.txt @@ -0,0 +1,7 @@ + +file(GLOB TEST_PROGS "test*.cpp") + +OpenSimAddTests( + TESTPROGRAMS ${TEST_PROGS} + LINKLIBS osimTools + ) diff --git a/OpenSim/Tests/ConstantCurvatureExample/testConstantCurvature.cpp b/OpenSim/Tests/ConstantCurvatureExample/testConstantCurvature.cpp new file mode 100644 index 0000000000..1d2e82627a --- /dev/null +++ b/OpenSim/Tests/ConstantCurvatureExample/testConstantCurvature.cpp @@ -0,0 +1,358 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testConstantCurvature.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2023 Stanford University and the Authors * + * Author(s): Keenon Werling * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +/* These tests cover checking the analytical Jacobians and gradients of + * Jacobians for the ConstantCurvatureJoint. The ground-truth Jacobians were + * computed using finite differences in Nimble, and then converted into + * hard-coded C++ by hand. + */ + +#include "OpenSim/Actuators/SpringGeneralizedForce.h" +#include "OpenSim/Auxiliary/auxiliaryTestFunctions.h" +#include "OpenSim/Simulation/Model/CoordinateLimitForce.h" +#include "OpenSim/Simulation/Model/Geometry.h" +#include +// #define VISUALIZE + +#include "OpenSim/OpenSim.h" + +using namespace SimTK; +using namespace OpenSim; + +void testJacobians1() { + using namespace SimTK; + + double d = 0.2; + Vec3 q(0.5, 0.5, 0.5); + Vec3 qDot(0.1, 0.2, 0.3); + + //////////////////////////////////////////////////////// + // Test the ordinary Jacobian + //////////////////////////////////////////////////////// + + // Jacobian: + // 0.7701511529 -0.4794255386 0.0000000000 + // -0.4794255386 0.0000000000 1.0000000000 + // 0.4207354924 0.8775825619 0.0000000000 + // -0.0388101162 -0.0864079951 0.0000000000 + // 0.0148489243 0.0148489243 0.0000000000 + // 0.0784030482 -0.0517875648 0.0000000000 + Mat63 expectedJacobian; + expectedJacobian.setToZero(); + expectedJacobian(0, 0) = 0.7701511529; + expectedJacobian(0, 1) = -0.4794255386; + expectedJacobian(0, 2) = 0.0000000000; + expectedJacobian(1, 0) = -0.4794255386; + expectedJacobian(1, 1) = 0.0000000000; + expectedJacobian(1, 2) = 1.0000000000; + expectedJacobian(2, 0) = 0.4207354924; + expectedJacobian(2, 1) = 0.8775825619; + expectedJacobian(2, 2) = 0.0000000000; + expectedJacobian(3, 0) = -0.0388101162; + expectedJacobian(3, 1) = -0.0864079951; + expectedJacobian(3, 2) = 0.0000000000; + expectedJacobian(4, 0) = 0.0148489243; + expectedJacobian(4, 1) = 0.0148489243; + expectedJacobian(4, 2) = 0.0000000000; + expectedJacobian(5, 0) = 0.0784030482; + expectedJacobian(5, 1) = -0.0517875648; + expectedJacobian(5, 2) = 0.0000000000; + + Mat63 J = ConstantCurvatureJoint::getConstantCurveJacobian(q, d); + + Mat63 diffJ = J - expectedJacobian; + ASSERT(diffJ.norm() < 1e-9, __FILE__, __LINE__, + "Jacobian didn't match expected value"); + + //////////////////////////////////////////////////////// + // Test the time derivative of the Jacobian + //////////////////////////////////////////////////////// + + // Jacobian: + // -0.2103677462 -0.2632747686 0.0000000000 + // -0.1755165124 0.0000000000 0.0000000000 + // 0.1850755765 -0.1438276616 0.0000000000 + // -0.0175753539 0.0167370896 0.0000000000 + // 0.0015923415 0.0051216193 0.0000000000 + // -0.0189190608 -0.0280126345 0.0000000000 + Mat63 expectedJacobianTimeDeriv; + expectedJacobianTimeDeriv.setToZero(); + expectedJacobianTimeDeriv(0, 0) = -0.2103677462; + expectedJacobianTimeDeriv(0, 1) = -0.2632747686; + expectedJacobianTimeDeriv(0, 2) = 0.0000000000; + expectedJacobianTimeDeriv(1, 0) = -0.1755165124; + expectedJacobianTimeDeriv(1, 1) = 0.0000000000; + expectedJacobianTimeDeriv(1, 2) = 0.0000000000; + expectedJacobianTimeDeriv(2, 0) = 0.1850755765; + expectedJacobianTimeDeriv(2, 1) = -0.1438276616; + expectedJacobianTimeDeriv(2, 2) = 0.0000000000; + expectedJacobianTimeDeriv(3, 0) = -0.0175753539; + expectedJacobianTimeDeriv(3, 1) = 0.0167370896; + expectedJacobianTimeDeriv(3, 2) = 0.0000000000; + expectedJacobianTimeDeriv(4, 0) = 0.0015923415; + expectedJacobianTimeDeriv(4, 1) = 0.0051216193; + expectedJacobianTimeDeriv(4, 2) = 0.0000000000; + expectedJacobianTimeDeriv(5, 0) = -0.0189190608; + expectedJacobianTimeDeriv(5, 1) = -0.0280126345; + expectedJacobianTimeDeriv(5, 2) = 0.0000000000; + + Mat63 Jdot = ConstantCurvatureJoint::getConstantCurveJacobianDerivWrtTime( + q, qDot, d); + + Mat63 diffJdot = Jdot - expectedJacobianTimeDeriv; + ASSERT(diffJdot.norm() < 1e-9, __FILE__, __LINE__, + "Jacobian time deriv didn't match expected value"); +} + +void testJacobians2() { + using namespace SimTK; + + double d = 1.0; + Vec3 q(0.50464766, 0.55085949, 0.0018320994); + Vec3 qDot(0.44693263, 0.76950436, 0.0065713527); + + //////////////////////////////////////////////////////// + // Test the ordinary Jacobian + //////////////////////////////////////////////////////// + + // Jacobian: + // 0.8520735327 -0.0018320984 0.0000000000 + // -0.5234197721 0.0000000000 1.0000000000 + // 0.0015610852 0.9999983217 0.0000000000 + // 0.0184228695 -0.5019869761 0.0000000000 + // 0.0731720241 0.0813768660 0.0000000000 + // 0.4257335679 -0.0234562723 0.0000000000 + Mat63 expectedJacobian; + expectedJacobian.setToZero(); + expectedJacobian(0, 0) = 0.8520735327; + expectedJacobian(0, 1) = -0.0018320984; + expectedJacobian(0, 2) = 0.0000000000; + expectedJacobian(1, 0) = -0.5234197721; + expectedJacobian(1, 1) = 0.0000000000; + expectedJacobian(1, 2) = 1.0000000000; + expectedJacobian(2, 0) = 0.0015610852; + expectedJacobian(2, 1) = 0.9999983217; + expectedJacobian(2, 2) = 0.0000000000; + expectedJacobian(3, 0) = 0.0184228695; + expectedJacobian(3, 1) = -0.5019869761; + expectedJacobian(3, 2) = 0.0000000000; + expectedJacobian(4, 0) = 0.0731720241; + expectedJacobian(4, 1) = 0.0813768660; + expectedJacobian(4, 2) = 0.0000000000; + expectedJacobian(5, 0) = 0.4257335679; + expectedJacobian(5, 1) = -0.0234562723; + expectedJacobian(5, 2) = 0.0000000000; + + Mat63 J = ConstantCurvatureJoint::getConstantCurveJacobian(q, d); + + Mat63 diffJ = J - expectedJacobian; + ASSERT(diffJ.norm() < 1e-9, __FILE__, __LINE__, + "Jacobian didn't match expected value"); + + //////////////////////////////////////////////////////// + // Test the time derivative of the Jacobian + //////////////////////////////////////////////////////// + + // Jacobian: + // -0.4027833792 -0.0065713417 0.0000000000 + // -0.6556753989 0.0000000000 0.0000000000 + // 0.0048613545 -0.0000120394 0.0000000000 + // 0.0320114385 -0.0017449897 0.0000000000 + // 0.0351757892 0.0935909521 0.0000000000 + // -0.1964663065 -0.0543846437 0.0000000000 + Mat63 expectedJacobianTimeDeriv; + expectedJacobianTimeDeriv.setToZero(); + expectedJacobianTimeDeriv(0, 0) = -0.4027833792; + expectedJacobianTimeDeriv(0, 1) = -0.0065713417; + expectedJacobianTimeDeriv(0, 2) = 0.0000000000; + expectedJacobianTimeDeriv(1, 0) = -0.6556753989; + expectedJacobianTimeDeriv(1, 1) = 0.0000000000; + expectedJacobianTimeDeriv(1, 2) = 0.0000000000; + expectedJacobianTimeDeriv(2, 0) = 0.0048613545; + expectedJacobianTimeDeriv(2, 1) = -0.0000120394; + expectedJacobianTimeDeriv(2, 2) = 0.0000000000; + expectedJacobianTimeDeriv(3, 0) = 0.0320114385; + expectedJacobianTimeDeriv(3, 1) = -0.0017449897; + expectedJacobianTimeDeriv(3, 2) = 0.0000000000; + expectedJacobianTimeDeriv(4, 0) = 0.0351757892; + expectedJacobianTimeDeriv(4, 1) = 0.0935909521; + expectedJacobianTimeDeriv(4, 2) = 0.0000000000; + expectedJacobianTimeDeriv(5, 0) = -0.1964663065; + expectedJacobianTimeDeriv(5, 1) = -0.0543846437; + expectedJacobianTimeDeriv(5, 2) = 0.0000000000; + + Mat63 Jdot = ConstantCurvatureJoint::getConstantCurveJacobianDerivWrtTime( + q, qDot, d); + + Mat63 diffJdot = Jdot - expectedJacobianTimeDeriv; + ASSERT(diffJdot.norm() < 1e-9, __FILE__, __LINE__, + "Jacobian time deriv didn't match expected value"); +} + +void testJacobians3() { + using namespace SimTK; + + double d = 1.0; + + Vec3 q(0.79846287622439227, 1.5707963210265892, -0.015968884371590844); + // Check that this doesn't fire any asserts or crash + Mat63 J = ConstantCurvatureJoint::getConstantCurveJacobian(q, d); + (void)J; // keep compiler from complaining +} + +int main() { + testJacobians1(); + testJacobians2(); + testJacobians3(); + + Model model; + model.setName("spring-stack"); +#ifdef VISUALIZE + model.setUseVisualizer(true); +#endif + + std::vector bodies; + std::vector joints; + + const int numBodies = 3; + + double springStiffness = 28.0; + double springViscosity = 2.0; + + // Create display geometry. + Brick bodyGeometry(Vec3(0.1, 0.1, 0.1)); + bodyGeometry.setColor(Gray); + + // Add a console reporter to print the muscle fiber force and elbow angle. + ConsoleReporter* reporter = new ConsoleReporter(); + reporter->set_report_time_interval(1.0); + + for (int i = 0; i < numBodies; i++) { + OpenSim::Body* body = new OpenSim::Body("body_" + std::to_string(i), 1, + Vec3(0), Inertia(0.1, 0.1, 0.1)); + + model.addBody(body); + bodies.push_back(body); + + // Attach an ellipsoid to a frame located at the center of each body. + PhysicalOffsetFrame* visualCenter = + new PhysicalOffsetFrame("body_viz_center_" + std::to_string(i), + *body, Transform(Vec3(0))); + body->addComponent(visualCenter); + visualCenter->attachGeometry(bodyGeometry.clone()); + + // Connect the bodies with pin joints. Assume each body is 1 m long. + ConstantCurvatureJoint* joint; + if (i > 0) { + joint = new ConstantCurvatureJoint("ccj" + std::to_string(i), + *(bodies[i - 1]), *(bodies[i]), Vec3(0, 0, 0), 1.0); + } else { + joint = new ConstantCurvatureJoint("ccj" + std::to_string(i), + model.getGround(), *(bodies[i]), Vec3(0, 0, 0), 1.0); + } + joint->updCoordinate(ConstantCurvatureJoint::Coord::RotationX) + .setName("joint_" + std::to_string(i) + "_x"); + joint->updCoordinate(ConstantCurvatureJoint::Coord::RotationZ) + .setName("joint_" + std::to_string(i) + "_z"); + joint->updCoordinate(ConstantCurvatureJoint::Coord::RotationY) + .setName("joint_" + std::to_string(i) + "_y"); + + SpringGeneralizedForce* springForceX = + new SpringGeneralizedForce("joint_" + std::to_string(i) + "_x"); + springForceX->setStiffness(springStiffness); + springForceX->setViscosity(springViscosity); + springForceX->setRestLength(0); + model.addForce(springForceX); + SpringGeneralizedForce* springForceZ = + new SpringGeneralizedForce("joint_" + std::to_string(i) + "_z"); + springForceZ->setStiffness(springStiffness); + springForceZ->setViscosity(springViscosity); + springForceZ->setRestLength(0); + model.addForce(springForceZ); + SpringGeneralizedForce* springForceY = + new SpringGeneralizedForce("joint_" + std::to_string(i) + "_y"); + springForceY->setStiffness(springStiffness); + springForceY->setViscosity(springViscosity); + springForceY->setRestLength(0); + model.addForce(springForceY); + + model.addJoint(joint); + joints.push_back(joint); + } + + reporter->addToReport( + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationX) + .getOutput("value"), + "X"); + reporter->addToReport( + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationZ) + .getOutput("value"), + "Z"); + reporter->addToReport( + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationY) + .getOutput("value"), + "Y"); + reporter->addToReport( + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationX) + .getOutput("speed"), + "vX"); + reporter->addToReport( + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationZ) + .getOutput("speed"), + "vZ"); + reporter->addToReport( + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationY) + .getOutput("speed"), + "vY"); + + model.addComponent(reporter); + + // Configure the model. + State& state = model.initSystem(); + + static_cast(joints[0]) + ->getCoordinate(ConstantCurvatureJoint::Coord::RotationZ) + .setValue(state, 0.01); + // static_cast(joints[0])->getCoordinate( + // ConstantCurvatureJoint::Coord::RotationX).setValue(state, + // 0.001); + + // Configure the visualizer. +#ifdef VISUALIZE + model.updMatterSubsystem().setShowDefaultGeometry(true); + Visualizer& viz = model.updVisualizer().updSimbodyVisualizer(); + viz.setBackgroundType(viz.SolidColor); + viz.setBackgroundColor(White); +#endif + + // Simulate. + simulate(model, state, 20.0); + + return 0; +};