| 114 | btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase); |
| 115 | |
| 116 | void InverseDynamicsExample::initPhysics() |
| 117 | { |
| 118 | //roboticists like Z up |
| 119 | int upAxis = 2; |
| 120 | m_guiHelper->setUpAxis(upAxis); |
| 121 | |
| 122 | createEmptyDynamicsWorld(); |
| 123 | btVector3 gravity(0, 0, 0); |
| 124 | // gravity[upAxis]=-9.8; |
| 125 | m_dynamicsWorld->setGravity(gravity); |
| 126 | |
| 127 | m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); |
| 128 | |
| 129 | { |
| 130 | SliderParams slider("Kp", &kp); |
| 131 | slider.m_minVal = 0; |
| 132 | slider.m_maxVal = 2000; |
| 133 | if (m_guiHelper->getParameterInterface()) |
| 134 | m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); |
| 135 | } |
| 136 | { |
| 137 | SliderParams slider("Kd", &kd); |
| 138 | slider.m_minVal = 0; |
| 139 | slider.m_maxVal = 50; |
| 140 | if (m_guiHelper->getParameterInterface()) |
| 141 | m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); |
| 142 | } |
| 143 | |
| 144 | if (m_option == BT_ID_PROGRAMMATICALLY) |
| 145 | { |
| 146 | ButtonParams button("toggle inverse model", 0, true); |
| 147 | button.m_callback = toggleUseInverseModel; |
| 148 | m_guiHelper->getParameterInterface()->registerButtonParameter(button); |
| 149 | } |
| 150 | |
| 151 | switch (m_option) |
| 152 | { |
| 153 | case BT_ID_LOAD_URDF: |
| 154 | { |
| 155 | BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0); |
| 156 | bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf"); // lwr / kuka.urdf"); |
| 157 | if (loadOk) |
| 158 | { |
| 159 | int rootLinkIndex = u2b.getRootLinkIndex(); |
| 160 | b3Printf("urdf root link index = %d\n", rootLinkIndex); |
| 161 | MyMultiBodyCreator creation(m_guiHelper); |
| 162 | btTransform identityTrans; |
| 163 | identityTrans.setIdentity(); |
| 164 | ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, true, u2b.getPathPrefix()); |
| 165 | for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) |
| 166 | { |
| 167 | m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); |
| 168 | } |
| 169 | m_multiBody = creation.getBulletMultiBody(); |
| 170 | if (m_multiBody) |
| 171 | { |
| 172 | //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator |
| 173 | //temporarily set some extreme damping factors until we have some joint control or constraints |
nothing calls this directly
no test coverage detected