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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
|
#ifndef SimTK_SIMBODY_EXAMPLE_ATLAS_H_
#define SimTK_SIMBODY_EXAMPLE_ATLAS_H_
/* -------------------------------------------------------------------------- *
* Simbody(tm) Example: Boston Dynamics Atlas robot *
* -------------------------------------------------------------------------- *
* This is part of the SimTK biosimulation toolkit originating from *
* Simbios, the NIH National Center for Physics-Based Simulation of *
* Biological Structures at Stanford, funded under the NIH Roadmap for *
* Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
* *
* Portions copyright (c) 2014 Stanford University and the Authors. *
* Authors: Michael Sherman *
* Contributors: Jack Wang, Chris Dembia, John Hsu *
* *
* 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 "Simbody.h"
#include "URDFReader.h"
/* This is a Simbody model of the Boston Dynamics Atlas humanoid robot.
The up direction is Z.
*/
class Atlas : public SimTK::MultibodySystem {
public:
// Should be just the file name and extension; will search in
// auxDir/models/ directory.
Atlas(const std::string& auxDir, const std::string& urdfFileName);
// Return the parsed-in version of the URDF specification.
const URDFRobot& getURDFRobot() const {return m_urdfRobot;}
void setAngleNoise(SimTK::State& state, SimTK::Real qNoise) const
{ m_qNoise.setValue(state, qNoise); }
void setRateNoise(SimTK::State& state, SimTK::Real uNoise) const
{ m_uNoise.setValue(state, uNoise); }
void setSampledAngles(SimTK::State& state, const SimTK::Vector& angles) const {
m_sampledAngles.setValue(state, angles);
}
void setSampledRates(SimTK::State& state, const SimTK::Vector& rates) const {
m_sampledRates.setValue(state, rates);
}
void setSampledPelvisPose(SimTK::State& state,
const SimTK::Transform& pose) const
{ m_sampledPelvisPose.setValue(state, pose); }
void setSampledEndEffectorPos(SimTK::State& state,
const SimTK::Vec3& pos) const
{ m_sampledEndEffectorPos.setValue(state, pos); }
SimTK::Real getAngleNoise(const SimTK::State& s) const
{ return m_qNoise.getValue(s); }
SimTK::Real getRateNoise(const SimTK::State& s) const
{ return m_uNoise.getValue(s); }
const SimTK::Vector& getSampledAngles(const SimTK::State& s) const
{ return m_sampledAngles.getValue(s); }
const SimTK::Vector& getSampledRates(const SimTK::State& s) const
{ return m_sampledRates.getValue(s); }
const SimTK::Transform& getSampledPelvisPose(const SimTK::State& s) const
{ return m_sampledPelvisPose.getValue(s); }
const SimTK::Vec3& getSampledEndEffectorPos(const SimTK::State& s) const
{ return m_sampledEndEffectorPos.getValue(s); }
const SimTK::Vector& getEffortLimits() const {return m_effortLimits;}
const SimTK::Vector& getSpeedLimits() const {return m_speedLimits;}
const SimTK::Vector& getLowerLimits() const {return m_lowerLimits;}
const SimTK::Vector& getUpperLimits() const {return m_upperLimits;}
// Return the Ground frame location of the body origin point of the
// EndEffector link.
SimTK::Vec3 getActualEndEffectorPosition(const SimTK::State& s) const
{ return getBody(m_endEffectorLinkName)
.findStationLocationInGround(s, m_endEffectorStation); }
// Set a particular joint angle (in radians) in the given State.
void setJointAngle
(SimTK::State& s, SimTK::QIndex which, SimTK::Real angle) const
{ s.updQ()[which] = angle; }
// Set a particular joint angular rate (in radians/s) in the given State.
void setJointRate
(SimTK::State& s, SimTK::UIndex which, SimTK::Real rate) const
{ s.updU()[which] = rate; }
const SimTK::SimbodyMatterSubsystem& getMatterSubsystem() const {return m_matter;}
SimTK::SimbodyMatterSubsystem& updMatterSubsystem() {return m_matter;}
const SimTK::GeneralForceSubsystem& getForceSubsystem() const {return m_forces;}
SimTK::GeneralForceSubsystem& updForceSubsystem() {return m_forces;}
const SimTK::Force::Gravity& getGravity() const {return m_gravity;}
/// Realizes the topology and model, and uses the resulting initial state
/// to perform further internal initialization. Sets up joint limit arrays.
void initialize(SimTK::State& state);
const SimTK::MobilizedBody& getBody(const std::string& name) const {
return m_urdfRobot.links.getLink(name).masterMobod;
}
SimTK::MobilizedBody& updBody(const std::string& name) {
return m_urdfRobot.links.updLink(name).masterMobod;
}
const SimTK::MobilizedBody& getEndEffectorBody() const {
return m_urdfRobot.links.getLink(m_endEffectorLinkName).masterMobod;
}
const SimTK::Vec3& getEndEffectorStation() const {
return m_endEffectorStation;
}
private:
SimTK::SimbodyMatterSubsystem m_matter;
SimTK::GeneralForceSubsystem m_forces;
SimTK::ContactTrackerSubsystem m_tracker;
SimTK::CompliantContactSubsystem m_contact;
SimTK::Force::Gravity m_gravity;
SimTK::Measure_<SimTK::Vector>::Variable m_sampledAngles;
SimTK::Measure_<SimTK::Vector>::Variable m_sampledRates;
SimTK::Measure_<SimTK::Transform>::Variable m_sampledPelvisPose;
SimTK::Measure_<SimTK::Vec3>::Variable m_sampledEndEffectorPos;
SimTK::Measure_<SimTK::Real>::Variable m_qNoise;
SimTK::Measure_<SimTK::Real>::Variable m_uNoise;
SimTK::MultibodyGraphMaker m_mbGraph;
URDFRobot m_urdfRobot;
std::string m_endEffectorLinkName;
SimTK::Vec3 m_endEffectorStation;
// Limits on generalized forces and velocities (nu)
SimTK::Vector m_effortLimits;
SimTK::Vector m_speedLimits;
// Limits on generalized coordinates (nq)
SimTK::Vector m_lowerLimits;
SimTK::Vector m_upperLimits;
};
#endif // SimTK_SIMBODY_EXAMPLE_ATLAS_H_
|
