1 #ifndef SimTK_SIMBODY_EXAMPLE_UR10_H_ 2 #define SimTK_SIMBODY_EXAMPLE_UR10_H_ 3 4 /* -------------------------------------------------------------------------- * 5 * Simbody(tm) Example: Universal Robotics UR10 Arm * 6 * -------------------------------------------------------------------------- * 7 * This is part of the SimTK biosimulation toolkit originating from * 8 * Simbios, the NIH National Center for Physics-Based Simulation of * 9 * Biological Structures at Stanford, funded under the NIH Roadmap for * 10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 11 * * 12 * Portions copyright (c) 2014 Stanford University and the Authors. * 13 * Authors: Michael Sherman * 14 * Contributors: Jack Wang, Chris Dembia, John Hsu * 15 * * 16 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 17 * not use this file except in compliance with the License. You may obtain a * 18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 19 * * 20 * Unless required by applicable law or agreed to in writing, software * 21 * distributed under the License is distributed on an "AS IS" BASIS, * 22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 23 * See the License for the specific language governing permissions and * 24 * limitations under the License. * 25 * -------------------------------------------------------------------------- */ 26 27 #include "Simbody.h" 28 29 /* This is a Simbody model of the Universal Robotics UR10 6-dof manipulator arm. 30 See http://www.universal-robots.com. 31 32 The up direction is Z. The model has the following bodies (links in robot 33 parlance) and joints. All joints are pin joints: 34 base welded to ground 35 shoulder shoulder_pan joint to base, about Z 36 upper_arm shoulder_lift joint to shoulder, about Y 37 forearm elbow joint to upper_arm, about Y 38 wrist_1 wrist_1 joint to forearm, about Y 39 wrist_2 wrist_2 joint to wrist_1, about Z 40 wrist_3 wrist_3 joint to wrist_2, about Y 41 ee end effector welded to wrist_3 42 43 There is a torque motor at each joint, with limits to the torque and maximum 44 speed. All angles are restricted to -2Pi .. 2Pi (per Universal Robotics spec 45 sheet). 46 Joint Tmax(N-m) Vmax(rad/s) 47 shoulder_pan 330 2.16 48 shoulder_lift 330 2.16 49 elbow 150 3.15 50 wrist_1,2,3 54 3.2 51 */ 52 53 class UR10 : public SimTK::MultibodySystem { 54 public: 55 // Pass in the directory to use for getting geometry/xxx mesh files. 56 explicit UR10(const std::string& auxDir); 57 58 enum Link { 59 Ground = 0, // These are MobilizedBody indices. 60 Base = 1, 61 Shoulder = 2, 62 UpperArm = 3, 63 Forearm = 4, 64 Wrist1 = 5, 65 Wrist2 = 6, 66 Wrist3 = 7, 67 EndEffector = 8 68 }; 69 static const int NumLinks = EndEffector + 1; // include ground 70 71 enum Coords { 72 PanCoord = 0, // These are the q and u coordinate indices 73 LiftCoord = 1, 74 ElbowCoord = 2, 75 Wrist1Coord = 3, 76 Wrist2Coord = 4, 77 Wrist3Coord = 5 78 }; 79 static const int NumCoords = Wrist3Coord + 1; 80 setAngleNoise(SimTK::State & state,SimTK::Real qNoise)81 void setAngleNoise(SimTK::State& state, SimTK::Real qNoise) const 82 { m_qNoise.setValue(state, qNoise); } 83 setRateNoise(SimTK::State & state,SimTK::Real uNoise)84 void setRateNoise(SimTK::State& state, SimTK::Real uNoise) const 85 { m_uNoise.setValue(state, uNoise); } 86 setSampledAngles(SimTK::State & state,const SimTK::Vector & angles)87 void setSampledAngles(SimTK::State& state, const SimTK::Vector& angles) const { 88 assert(angles.size() == NumCoords); 89 m_sampledAngles.setValue(state, angles); 90 } 91 setSampledRates(SimTK::State & state,const SimTK::Vector & rates)92 void setSampledRates(SimTK::State& state, const SimTK::Vector& rates) const { 93 assert(rates.size() == NumCoords); 94 m_sampledRates.setValue(state, rates); 95 } 96 setSampledEndEffectorPos(SimTK::State & state,const SimTK::Vec3 & pos)97 void setSampledEndEffectorPos(SimTK::State& state, 98 const SimTK::Vec3& pos) const 99 { m_sampledEndEffectorPos.setValue(state, pos); } 100 getAngleNoise(const SimTK::State & s)101 SimTK::Real getAngleNoise(const SimTK::State& s) const 102 { return m_qNoise.getValue(s); } getRateNoise(const SimTK::State & s)103 SimTK::Real getRateNoise(const SimTK::State& s) const 104 { return m_uNoise.getValue(s); } getSampledAngles(const SimTK::State & s)105 const SimTK::Vector& getSampledAngles(const SimTK::State& s) const 106 { return m_sampledAngles.getValue(s); } getSampledRates(const SimTK::State & s)107 const SimTK::Vector& getSampledRates(const SimTK::State& s) const 108 { return m_sampledRates.getValue(s); } getSampledEndEffectorPos(const SimTK::State & s)109 const SimTK::Vec3& getSampledEndEffectorPos(const SimTK::State& s) const 110 { return m_sampledEndEffectorPos.getValue(s); } 111 112 // Return the Ground frame location of the body origin point of the 113 // EndEffector link. getActualEndEffectorPosition(const SimTK::State & s)114 SimTK::Vec3 getActualEndEffectorPosition(const SimTK::State& s) const 115 { return m_bodies[EndEffector].getBodyOriginLocation(s); } 116 117 // Set a particular joint angle (in radians) in the given State. setJointAngle(SimTK::State & s,Coords which,SimTK::Real angle)118 void setJointAngle(SimTK::State& s, Coords which, SimTK::Real angle) const 119 { s.updQ()[which] = angle; } 120 121 // Set a particular joint angular rate (in radians/s) in the given State. setJointRate(SimTK::State & s,Coords which,SimTK::Real rate)122 void setJointRate(SimTK::State& s, Coords which, SimTK::Real rate) const 123 { s.updU()[which] = rate; } 124 125 // Given a set of proposed actuator torques tau, make sure each is in 126 // range given the specifications for each actuator. clampToLimits(SimTK::Vector & tau)127 static void clampToLimits(SimTK::Vector& tau) { 128 SimTK::clampInPlace(-330, tau[PanCoord], 330); 129 SimTK::clampInPlace(-330, tau[LiftCoord], 330); 130 SimTK::clampInPlace(-150, tau[ElbowCoord], 150); 131 SimTK::clampInPlace( -54, tau[Wrist1Coord], 54); 132 SimTK::clampInPlace( -54, tau[Wrist2Coord], 54); 133 SimTK::clampInPlace( -54, tau[Wrist3Coord], 54); 134 } 135 getMatterSubsystem()136 const SimTK::SimbodyMatterSubsystem& getMatterSubsystem() const {return m_matter;} updMatterSubsystem()137 SimTK::SimbodyMatterSubsystem& updMatterSubsystem() {return m_matter;} 138 getForceSubsystem()139 const SimTK::GeneralForceSubsystem& getForceSubsystem() const {return m_forces;} updForceSubsystem()140 SimTK::GeneralForceSubsystem& updForceSubsystem() {return m_forces;} 141 getGravity()142 const SimTK::Force::Gravity& getGravity() const {return m_gravity;} 143 144 /// Realizes the topology and model, and uses the resulting initial state 145 /// to perform further internal initialization. initialize(SimTK::State & state)146 void initialize(SimTK::State& state) { 147 state = realizeTopology(); 148 realizeModel(state); 149 } 150 getBody(Link linkno)151 const SimTK::MobilizedBody& getBody(Link linkno) const { 152 return m_bodies[linkno]; 153 } 154 155 private: 156 SimTK::SimbodyMatterSubsystem m_matter; 157 SimTK::GeneralForceSubsystem m_forces; 158 SimTK::Force::Gravity m_gravity; 159 SimTK::MobilizedBody m_bodies[NumLinks]; 160 SimTK::Measure_<SimTK::Vector>::Variable m_sampledAngles; 161 SimTK::Measure_<SimTK::Vector>::Variable m_sampledRates; 162 SimTK::Measure_<SimTK::Vec3>::Variable m_sampledEndEffectorPos; 163 SimTK::Measure_<SimTK::Real>::Variable m_qNoise; 164 SimTK::Measure_<SimTK::Real>::Variable m_uNoise; 165 }; 166 167 168 169 #endif // SimTK_SIMBODY_EXAMPLE_UR10_H_ 170