Skip to content

Commit

Permalink
Update properties.
Browse files Browse the repository at this point in the history 
Conform to (Model)Component interface.
  • Loading branch information
aseth1 committed Oct 14, 2015
1 parent a629ff6 commit d0a0b06
Show file tree
Hide file tree
Showing 2 changed files with 136 additions and 216 deletions.
221 changes: 78 additions & 143 deletions ScapulothoracicJoint.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,25 +1,25 @@
/* -------------------------------------------------------------------------- *
* OpenSim: ScapulothoracicJoint.cpp *
* -------------------------------------------------------------------------- *
* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
* See http:https://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-2012 Stanford University and the Authors *
* Author(s): Ajay Seth *
* *
* 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:https://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. *
* -------------------------------------------------------------------------- */
* OpenSim: ScapulothoracicJoint.cpp *
* -------------------------------------------------------------------------- *
* ScapulothoracicJoint is offered as an addition to the OpenSim API *
* See http:https://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-2013 Stanford University and the Authors *
* Author(s): Ajay Seth *
* *
* 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:https://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
Expand All @@ -36,63 +36,35 @@ using namespace SimTK;
using namespace OpenSim;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
// CONSTRUCTOR(S)
//=============================================================================
//_____________________________________________________________________________
/**
* Destructor.
*/
ScapulothoracicJoint::~ScapulothoracicJoint()
{
}
//_____________________________________________________________________________
/**
/*
* Default constructor.
*/
ScapulothoracicJoint::ScapulothoracicJoint() :
Joint(),
_ellipsoidRadii(_ellipsoidRadiiProp.getValueDblVec()),
_wingOrigin(_wingOriginProp.getValueDblVec()),
_wingDirection(_wingDirectionProp.getValueDbl())
{
setNull();
setupProperties();
}
//_____________________________________________________________________________
/**
* Copy constructor.
*
* @param aJoint ScapulothoracicJoint to be copied.
*/
ScapulothoracicJoint::ScapulothoracicJoint(const ScapulothoracicJoint &aJoint) :
Joint(aJoint),
_ellipsoidRadii(_ellipsoidRadiiProp.getValueDblVec()),
_wingOrigin(_wingOriginProp.getValueDblVec()),
_wingDirection(_wingDirectionProp.getValueDbl())
ScapulothoracicJoint::ScapulothoracicJoint() : Joint()
{
setNull();
setupProperties();
copyData(aJoint);
constructCoordinates();
constructProperties();
}


//_____________________________________________________________________________
/**
* Convenience Constructor.
*/
ScapulothoracicJoint::ScapulothoracicJoint(const std::string &name, OpenSim::Body& parent, SimTK::Vec3 locationInParent, SimTK::Vec3 orientationInParent,
OpenSim::Body& body, SimTK::Vec3 locationInBody, SimTK::Vec3 orientationInBody,
SimTK::Vec3 ellipsoidRadii, SimTK::Vec2 wingingOrigin, double wingingDirection, bool reverse) :
Joint(name, parent, locationInParent,orientationInParent, body, locationInBody, orientationInBody, reverse),
_ellipsoidRadii(_ellipsoidRadiiProp.getValueDblVec()),
_wingOrigin(_wingOriginProp.getValueDblVec()),
_wingDirection(_wingDirectionProp.getValueDbl())
Joint(name, parent, locationInParent,orientationInParent, body, locationInBody, orientationInBody, reverse)
{
setNull();
setupProperties();
constructCoordinates();
constructProperties();

_ellipsoidRadii = ellipsoidRadii;
_wingOrigin = wingingOrigin;
_wingDirection = wingingDirection;
upd_thoracic_ellipsoid_radii_x_y_z() = ellipsoidRadii;
upd_scapula_winging_axis_origin(0) = wingingOrigin[0];
upd_scapula_winging_axis_origin(1) = wingingOrigin[1];
upd_scapula_winging_axis_direction() = wingingDirection;
updBody().setJoint(*this);
}

Expand All @@ -101,55 +73,21 @@ ScapulothoracicJoint::ScapulothoracicJoint(const std::string &name, OpenSim::Bod
//=============================================================================
//_____________________________________________________________________________
/**
* Copy this body and return a pointer to the copy.
* The copy constructor for this class is used.
*
* @return Pointer to a copy of this OpenSim::Body.
*/
Object* ScapulothoracicJoint::copy() const
{
ScapulothoracicJoint *joint = new ScapulothoracicJoint(*this);
return(joint);
}
//_____________________________________________________________________________
/**
* Copy data members from one ScapulothoracicJoint to another.
*
* @param aJoint ScapulothoracicJoint to be copied.
*/
void ScapulothoracicJoint::copyData(const ScapulothoracicJoint &aJoint)
{
_ellipsoidRadii = aJoint._ellipsoidRadii;
_wingOrigin = aJoint._wingOrigin;
_wingDirection= aJoint._wingDirection;
}

//_____________________________________________________________________________
/**
* Set the data members of this ScapulothoracicJoint to their null values.
*/
void ScapulothoracicJoint::setNull()
* Construct properties with their default values.
*/
void ScapulothoracicJoint::constructProperties()
{
constructCoordinates();
setAuthors("Ajay Seth");
Vec3 radii(NaN);
Vector origin(2, 0.0);
double dir = 0;

origin.size();
constructProperty_thoracic_ellipsoid_radii_x_y_z(radii);
constructProperty_scapula_winging_axis_origin(origin);
constructProperty_scapula_winging_axis_direction(dir);
}

//_____________________________________________________________________________
/**
* Connect properties to local pointers.
*/
void ScapulothoracicJoint::setupProperties()
{
_ellipsoidRadiiProp.setName("thoracic_ellipsoid_radii_x_y_z");
_propertySet.append(&_ellipsoidRadiiProp);

_wingOriginProp.setName("scapula_winging_axis_origin");
_wingOriginProp.setComment("Winging axis origin in the scapula plane (tangent to the thoracic surface).");
_propertySet.append(&_wingOriginProp);

_wingDirectionProp.setName("scapula_winging_axis_direction");
_wingDirectionProp.setComment("Winging axis orientation (in radians) in the scapula plane (tangent to the thoracic surface).");
_propertySet.append(&_wingDirectionProp);
}


//_____________________________________________________________________________
Expand Down Expand Up @@ -196,23 +134,7 @@ void ScapulothoracicJoint::connectToModel(Model& model)
}

// Base class checks that parent is valid
Joint::connectToModel(model);
}

//=============================================================================
// OPERATORS
//=============================================================================
//_____________________________________________________________________________
/**
* Assignment operator.
*
* @return Reference to this object.
*/
ScapulothoracicJoint& ScapulothoracicJoint::operator=(const ScapulothoracicJoint &aJoint)
{
Joint::operator=(aJoint);
copyData(aJoint);
return(*this);
Super::connectToModel(model);
}


Expand Down Expand Up @@ -249,7 +171,7 @@ void ScapulothoracicJoint::scale(const ScaleSet& aScaleSet)

for(int i=0; i<3; i++){
// Scale the size of the mobilizer
_ellipsoidRadii[i] *= scaleFactors[i];
upd_thoracic_ellipsoid_radii_x_y_z()[i] *= scaleFactors[i];
}
}

Expand Down Expand Up @@ -285,59 +207,72 @@ void ScapulothoracicJoint::addToSystem(SimTK::MultibodySystem& system) const
// CREATE MOBILIZED BODY
// Ellipsoid is rotated Pi/2 for desired rotations, but user's still wants to define Ellipsoid shape w.r.t thorax
// Swap ellipsoidRadii X,Y,Z in Thorax body frame to Y, X, Z in rotated joint frame in parent
Vec3 ellipsoidRadii(_ellipsoidRadii[1], _ellipsoidRadii[0], _ellipsoidRadii[2]);
Vec3 ellipsoidRadii(get_thoracic_ellipsoid_radii_x_y_z()[1],
get_thoracic_ellipsoid_radii_x_y_z()[0],
get_thoracic_ellipsoid_radii_x_y_z()[2]);
MobilizedBody::Ellipsoid
simtkMasslessBody(mutableThis->updModel().updMatterSubsystem().updMobilizedBody(getParentBody().getIndex()),
ellipsoidJointFrameInThorax, SimTK::Body::Massless(), ellipsoidJointFrameInIntermediate, ellipsoidRadii);
ellipsoidJointFrameInThorax, SimTK::Body::Massless(),
ellipsoidJointFrameInIntermediate,
ellipsoidRadii);

// get unit vector version of direction in the scapula joint frame of the Ellipsoid
// where the joint Z-axis is normal to the ellipsoid surface, X-axis is in the
// direction of abduction and Y is elevation in the neutral position
// winging is orthogonal to internal rotation (about Z) with axis is in XY-plane
// winging is orthogonal to upward rotation (about Z) with axis in XY-plane
// winging direction for 0 is aligned with intermediate frame Y and rotates
// counterclockwise with increasing angles.
SimTK::UnitVec3 dir(-sin(_wingDirection), cos(_wingDirection), 0);
const double& wingDirection = get_scapula_winging_axis_direction();
SimTK::UnitVec3 dir(-sin(wingDirection), cos(wingDirection), 0);

// Find rotation that aligns z-axis of pin mobilizer frame to winging axis
// This is in the scapula-ellipsoid (massless body) frame
SimTK::Rotation wingOrientationInIntermediateFrame(dir, ZAxis);

// origin of the winging axis w.r.t to the scapula joint frame of the ellipsoid
SimTK::Vec3 wingOriginInIntermediateFrame(_wingOrigin[0], _wingOrigin[1], 0);
SimTK::Vec3 wingOriginInIntermediateFrame(
get_scapula_winging_axis_origin(0), get_scapula_winging_axis_origin(1), 0);
// winging joint transform in the scapula ellipsoid joint frame
SimTK::Transform wingingInIntermediateFrame(wingOrientationInIntermediateFrame, wingOriginInIntermediateFrame);
SimTK::Transform wingingInIntermediateFrame(
wingOrientationInIntermediateFrame,
wingOriginInIntermediateFrame);

MobilizedBody::Pin simtkBody(simtkMasslessBody, wingingInIntermediateFrame, SimTK::Body::Massless(), wingingInIntermediateFrame);
MobilizedBody::Pin simtkBody(simtkMasslessBody,
wingingInIntermediateFrame, SimTK::Body::Massless(),
wingingInIntermediateFrame);

// Define the scapular joint frame in w.r.t to the Scapula body frame
Rotation rotation2(BodyRotationSequence, orientation[0],XAxis, orientation[1],YAxis, orientation[2], ZAxis);
Rotation rotation2(BodyRotationSequence, orientation[0], XAxis,
orientation[1], YAxis, orientation[2], ZAxis);
SimTK::Transform jointInScapula(rotation2, location);
MobilizedBody::Weld simtkBody2(simtkBody, Transform(), SimTK::Body::Rigid(mutableThis->updBody().getMassProperties()), jointInScapula);
MobilizedBody::Weld simtkBody2(simtkBody, Transform(),
SimTK::Body::Rigid(mutableThis->updBody().getMassProperties()),
jointInScapula);

setMobilizedBodyIndex(&mutableThis->updBody(), simtkBody2.getMobilizedBodyIndex());
setMobilizedBodyIndex(&mutableThis->updBody(),
simtkBody2.getMobilizedBodyIndex());

// Each coordinate needs to know it's body index and mobility index.
const CoordinateSet& coordinateSet = get_CoordinateSet();

// Link the coordinates for this Joint to the underlying mobilities
for(int i =0; i < _numMobilities; i++){
for(int i =0; i < _numMobilities; ++i){
Coordinate &coord = coordinateSet[i];
// Translations enabled by Translation mobilizer, first, but appear second in coordinate set
setCoordinateMobilizedBodyIndex(&coord, ((i < 3) ? (simtkMasslessBody.getMobilizedBodyIndex()) : (simtkBody.getMobilizedBodyIndex())));
setCoordinateMobilizedBodyIndex(&coord, ((i < 3) ?
(simtkMasslessBody.getMobilizedBodyIndex()) :
(simtkBody.getMobilizedBodyIndex())));
// The mobility index is the same as the order in which the coordinate appears in the coordinate set.
setCoordinateMobilizerQIndex(&coord, (i < 3 ? i : i-3));
}
}

void ScapulothoracicJoint::initStateFromProperties(SimTK::State& s) const
{
Joint::initStateFromProperties(s);
Super::initStateFromProperties(s);
}

void ScapulothoracicJoint::setPropertiesFromState(const SimTK::State& state)
{
const MultibodySystem& system = _model->getMultibodySystem();
const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

Joint::setPropertiesFromState(state);
Super::setPropertiesFromState(state);
}
Loading

0 comments on commit d0a0b06

Please sign in to comment.