1 /* -------------------------------------------------------------------------- *
2 * OpenSim: BallJoint.cpp *
3 * -------------------------------------------------------------------------- *
4 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
5 * See http://opensim.stanford.edu and the NOTICE file for more information. *
6 * OpenSim is developed at Stanford University and supported by the US *
7 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
8 * through the Warrior Web program. *
9 * *
10 * Copyright (c) 2005-2017 Stanford University and the Authors *
11 * Author(s): Ajay Seth *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 //=============================================================================
25 // INCLUDES
26 //=============================================================================
27 #include "BallJoint.h"
28 #include <OpenSim/Simulation/Model/Model.h>
29 #include "simbody/internal/MobilizedBody_Ball.h"
30
31 //=============================================================================
32 // STATICS
33 //=============================================================================
34 using namespace std;
35 using namespace SimTK;
36 using namespace OpenSim;
37
38 //=============================================================================
39 // Simbody Model building.
40 //=============================================================================
41 //_____________________________________________________________________________
extendAddToSystem(SimTK::MultibodySystem & system) const42 void BallJoint::extendAddToSystem(SimTK::MultibodySystem& system) const
43 {
44 Super::extendAddToSystem(system);
45 createMobilizedBody<MobilizedBody::Ball>(system);
46 }
47
extendInitStateFromProperties(SimTK::State & s) const48 void BallJoint::extendInitStateFromProperties(SimTK::State& s) const
49 {
50 Super::extendInitStateFromProperties(s);
51
52 const MultibodySystem& system = getModel().getMultibodySystem();
53 const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
54 if (matter.getUseEulerAngles(s))
55 return;
56
57 double xangle = getCoordinate(BallJoint::Coord::Rotation1X).getDefaultValue();
58 double yangle = getCoordinate(BallJoint::Coord::Rotation2Y).getDefaultValue();
59 double zangle = getCoordinate(BallJoint::Coord::Rotation3Z).getDefaultValue();
60 Rotation r(BodyRotationSequence, xangle, XAxis, yangle, YAxis, zangle, ZAxis);
61 //BallJoint* mutableThis = const_cast<BallJoint*>(this);
62 getChildFrame().getMobilizedBody().setQToFitRotation(s, r);
63 }
64
extendSetPropertiesFromState(const SimTK::State & state)65 void BallJoint::extendSetPropertiesFromState(const SimTK::State& state)
66 {
67 Super::extendSetPropertiesFromState(state);
68
69 // Override default behavior in case of quaternions.
70 const MultibodySystem& system = _model->getMultibodySystem();
71 const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
72 if (!matter.getUseEulerAngles(state)) {
73 Rotation r = getChildFrame().getMobilizedBody().getBodyRotation(state);
74 Vec3 angles = r.convertRotationToBodyFixedXYZ();
75
76 updCoordinate(BallJoint::Coord::Rotation1X).setDefaultValue(angles[0]);
77 updCoordinate(BallJoint::Coord::Rotation2Y).setDefaultValue(angles[1]);
78 updCoordinate(BallJoint::Coord::Rotation3Z).setDefaultValue(angles[2]);
79 }
80 }
81