BulletPhysicsWorld.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarXSimulation::Simulator
19  * @author Nikolaus ( vahrenkamp at kit dot edu )
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "BulletPhysicsWorld.h"
26 
27 #include <SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h>
28 #include <VirtualRobot/Robot.h>
29 #include <VirtualRobot/Nodes/ForceTorqueSensor.h>
30 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
31 #include <VirtualRobot/RobotFactory.h>
32 
33 #include <ArmarXCore/core/logging/Logging.h>
34 #include <ArmarXCore/core/system/ArmarXDataPath.h>
35 #include <algorithm>
36 
37 #include <memory>
38 #include <ArmarXCore/core/exceptions/LocalException.h>
39 
40 using namespace VirtualRobot;
41 using namespace SimDynamics;
42 using namespace armarx;
43 
44 
45 BulletPhysicsWorld::BulletPhysicsWorld() : SimulatedWorld()
46 {
47  synchronizeDurationMS = 0.0f;
48  currentSimTimeSec = 0.0;
49  collectContacts = false;
50 }
51 
52 BulletPhysicsWorld::~BulletPhysicsWorld()
53 {
54  bulletEngine.reset();
55  dynamicsWorld.reset();
56 }
57 
58 void BulletPhysicsWorld::initialize(int stepSizeMS, int bulletFixedTimeStepMS, int bulletFixedTimeStepMaxNrLoops, float maxRealTimeSimSpeed, bool floorPlane, std::string floorTexture)
59 {
60  ARMARX_INFO_S << "Init BulletPhysicsWorld. Creating bullet engine...";
61 
62  simVisuData.robots.clear();
63  simVisuData.objects.clear();
64  simVisuData.floorTextureFile.clear();
65  simVisuData.floor = false;
66 
67  simReportData.robots.clear();
68 
69  contacts.clear();
70  this->maxRealTimeSimSpeed = maxRealTimeSimSpeed;
71  this->stepSizeMs = stepSizeMS;
72  this->bulletFixedTimeStepMS = bulletFixedTimeStepMS;
73  this->bulletFixedTimeStepMaxNrLoops = bulletFixedTimeStepMaxNrLoops;
74 
75  dynamicsWorld = SimDynamics::DynamicsWorld::Init();
76 
77  bulletEngine = std::dynamic_pointer_cast<BulletEngine>(dynamicsWorld->getEngine());
78 
79  if (!bulletEngine)
80  {
81  ARMARX_ERROR_S << "Could not cast engine to BULLET engine, non-bullet engines not yet supported!";
82  throw UserException("Bullet engine missing");
83  }
84 
85  bulletEngine->setMutex(engineMutex);
86 
87  setupFloor(floorPlane, floorTexture);
88 
89  m_clock.reset();
90 }
91 
92 
93 void BulletPhysicsWorld::actuateRobotJoints(const std::string& robotName, const std::map< std::string, float>& angles, const std::map< std::string, float>& velocities)
94 {
95  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
96 
97  ARMARX_DEBUG_S << "actuateRobotJoints: first:" << angles.begin()->first << ", ang: " << angles.begin()->second << ", vel:" << velocities.begin()->second;
98  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
99  if (!dynamicsRobot)
100  {
101  ARMARX_WARNING_S << "No robot available";
102  return;
103  }
104 
105  RobotPtr kinRob = dynamicsRobot->getRobot();
106 
107  if (!kinRob || kinRob->getName() != robotName)
108  {
109  ARMARX_WARNING_S << "No robot available with name " << robotName;
110 
111  if (kinRob)
112  {
113  ARMARX_WARNING_S << "available robot:" << kinRob->getName();
114  }
115 
116  return;
117  }
118 
119  bool isStatic = dynamicRobots[robotName].isStatic;
120 
121  for (const auto& angle : angles)
122  {
123  // target values
124  std::string targetJointName = angle.first;
125  float targetJointPos = angle.second;
126  auto velIt = velocities.find(angle.first);
127 
128  if (isStatic)
129  {
130  RobotNodePtr rn = kinRob->getRobotNode(targetJointName);
131  BulletRobot* br = dynamic_cast<BulletRobot*>(dynamicsRobot.get());
132  if (br && br->hasLink(rn))
133  {
134  kinRob->setJointValue(rn, targetJointPos);
135  BulletRobot::LinkInfo link = br->getLink(rn);
136  RobotNodePtr nodeCloneA;
137  RobotNodePtr nodeCloneB;
138 
139  if (link.nodeA && link.dynNode1)
140  {
141  nodeCloneA = kinRob->getRobotNode(link.nodeA->getName());
142  link.dynNode1->setPose(nodeCloneA->getGlobalPose());
143  }
144  if (link.nodeB && link.dynNode2)
145  {
146  nodeCloneB = kinRob->getRobotNode(link.nodeB->getName());
147  link.dynNode1->setPose(nodeCloneB->getGlobalPose());
148  }
149  }
150  }
151  else
152  {
153  if (kinRob->hasRobotNode(targetJointName) && dynamicsRobot->getDynamicsRobotNode(targetJointName) && velIt != velocities.end())
154  {
155  VR_ASSERT(not kinRob->isPassive());
156  dynamicsRobot->actuateNode(dynamicsRobot->getRobot()->getRobotNode(targetJointName),
157  static_cast<double>(targetJointPos),
158  static_cast<double>(velIt->second));
159  }
160  else
161  {
162  ARMARX_WARNING_S << deactivateSpam(5, targetJointName) << "No node with name " << targetJointName;
163  }
164  }
165  }
166 }
167 
168 void BulletPhysicsWorld::actuateRobotJointsPos(const std::string& robotName, const std::map< std::string, float>& angles)
169 {
170  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
171  if (angles.size() == 0)
172  {
173  return;
174  }
175  ARMARX_DEBUG_S << "actuateRobotJointsPos: first:" << angles.begin()->first << ", ang: " << angles.begin()->second;
176 
177  // ARMARX_WARNING_S << "actuateRobotJointsPos: " << VAROUT(angles);
178 
179  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
180  if (!dynamicsRobot)
181  {
182  ARMARX_WARNING_S << "No robot available with name " << robotName;
183  return;
184  }
185  bool isStatic = dynamicRobots[robotName].isStatic;
186 
187  RobotPtr kinRob = dynamicsRobot->getRobot();
188 
189  if (!kinRob || kinRob->getName() != robotName)
190  {
191  ARMARX_WARNING_S << "No robot available with name " << robotName;
192  return;
193  }
194 
195  for (const auto& angle : angles)
196  {
197  // target values
198  std::string targetJointName = angle.first;
199  float targetJoint = angle.second;
200 
201  if (kinRob->hasRobotNode(targetJointName) /*&& dynamicsRobot->getDynamicsRobotNode(targetJointName)*/)
202  {
203  if (isStatic or kinRob->isPassive())
204  {
205  RobotNodePtr rn = kinRob->getRobotNode(targetJointName);
206  BulletRobot* br = dynamic_cast<BulletRobot*>(dynamicsRobot.get());
207  if (br && br->hasLink(rn))
208  {
209  kinRob->setJointValue(rn, targetJoint);
210  BulletRobot::LinkInfo link = br->getLink(rn);
211  RobotNodePtr nodeCloneA;
212  RobotNodePtr nodeCloneB;
213 
214  if (link.nodeA && link.dynNode1)
215  {
216  nodeCloneA = kinRob->getRobotNode(link.nodeA->getName());
217  link.dynNode1->setPose(nodeCloneA->getGlobalPose());
218  }
219  if (link.nodeB && link.dynNode2)
220  {
221  nodeCloneB = kinRob->getRobotNode(link.nodeB->getName());
222  link.dynNode1->setPose(nodeCloneB->getGlobalPose());
223  }
224  }
225  }
226  else
227  {
228  VR_ASSERT(not kinRob->isPassive());
229  dynamicsRobot->actuateNode(targetJointName,
230  static_cast<double>(targetJoint));
231  }
232  }
233  else
234  {
235  ARMARX_WARNING_S << deactivateSpam(5, targetJointName) << "No node with name " << targetJointName;
236  }
237  }
238 }
239 
240 void BulletPhysicsWorld::actuateRobotJointsVel(const std::string& robotName, const std::map< std::string, float>& velocities)
241 {
242  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
243  ARMARX_DEBUG_S << "actuateRobotJointsVel: first: vel:" << velocities.begin()->second;
244 
245  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
246  if (!dynamicsRobot)
247  {
248  ARMARX_WARNING_S << "No robot available";
249  return;
250  }
251 
252  RobotPtr kinRob = dynamicsRobot->getRobot();
253 
254  if (!kinRob || kinRob->getName() != robotName)
255  {
256  ARMARX_WARNING_S << "No robot available with name " << robotName;
257  return;
258  }
259 
260  // store velocities for static robots
261  DynamicsRobotInfo& ri = dynamicRobots[robotName];
262  if (ri.isStatic)
263  {
264  // preserve all values of velocities, and add values of robotInfo
265  std::map<std::string, float> res = velocities;
266  res.insert(ri.targetVelocities.begin(), ri.targetVelocities.end());
267  ri.targetVelocities = res;
268  }
269  else
270  {
271 
272  for (const auto& velocitie : velocities)
273  {
274  // target values
275  std::string targetJointName = velocitie.first;
276  float targetJointVel = velocitie.second;
277 
278  if (kinRob->hasRobotNode(targetJointName))
279  {
280  VR_ASSERT(not kinRob->isPassive());
281 
282  dynamicsRobot->actuateNodeVel(targetJointName, static_cast<double>(targetJointVel));
283  }
284  else
285  {
286  ARMARX_WARNING_S << "No node with name " << targetJointName;
287  }
288  }
289  }
290 }
291 
292 void BulletPhysicsWorld::actuateRobotJointsTorque(const std::string& robotName, const std::map< std::string, float>& torques)
293 {
294  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
295  ARMARX_DEBUG_S << "actuateRobotJointsTorque: first:" << torques.begin()->first << "," << torques.begin()->second;
296 
297  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
298  if (!dynamicsRobot)
299  {
300  ARMARX_WARNING_S << "No robot available";
301  return;
302  }
303 
304  RobotPtr kinRob = dynamicsRobot->getRobot();
305 
306  if (!kinRob || kinRob->getName() != robotName)
307  {
308  ARMARX_WARNING_S << "No robot available with name " << robotName;
309  return;
310  }
311 
312  for (const auto& torque : torques)
313  {
314  // target values
315  std::string targetJointName = torque.first;
316  float targetJointTorque = torque.second;
317 
318  if (kinRob->hasRobotNode(targetJointName))
319  {
320  VR_ASSERT(not kinRob->isPassive());
321 
322  dynamicsRobot->actuateNodeTorque(targetJointName, static_cast<double>(targetJointTorque));
323  }
324  else
325  {
326  ARMARX_WARNING_S << "No node with name " << targetJointName;
327  }
328  }
329 }
330 
331 void BulletPhysicsWorld::setRobotPose(const std::string& robotName, const Eigen::Matrix4f& globalPose)
332 {
333  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
334  ARMARX_DEBUG_S << "setRobotPose:" << globalPose(0, 3) << "," << globalPose(1, 3) << ", " << globalPose(2, 3);
335 
336  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
337  if (!dynamicsRobot)
338  {
339  ARMARX_WARNING_S << "No robot available";
340  return;
341  }
342 
343  RobotPtr kinRob = dynamicsRobot->getRobot();
344 
345  if (!kinRob || kinRob->getName() != robotName)
346  {
347  ARMARX_WARNING_S << "No robot available with name " << robotName;
348  return;
349  }
350 
351  Eigen::Matrix4f gp = globalPose; // just a hack to convert const to non-const (fixed in newest simox version)
352  dynamicsRobot->setGlobalPose(gp);
353 }
354 
355 void BulletPhysicsWorld::applyForceRobotNode(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& force)
356 {
357  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
358  ARMARX_DEBUG_S << "ApplyForce";
359 
360  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
361  if (!dynamicsRobot)
362  {
363  ARMARX_WARNING_S << "No robot available";
364  return;
365  }
366 
367  RobotPtr kinRob = dynamicsRobot->getRobot();
368 
369  if (!kinRob || kinRob->getName() != robotName)
370  {
371  ARMARX_WARNING_S << "No robot available with name " << robotName;
372  return;
373  }
374 
375  RobotNodePtr rn = kinRob->getRobotNode(robotNodeName);
376 
377  if (!rn)
378  {
379  ARMARX_WARNING_S << "No robotnode available with name " << robotNodeName;
380  return;
381  }
382 
383  dynamicsRobot->getDynamicsRobotNode(rn)->applyForce(force);
384 }
385 
386 void BulletPhysicsWorld::applyTorqueRobotNode(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& torque)
387 {
388  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
389  ARMARX_DEBUG_S << "ApplyTorque";
390 
391  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
392 
393  if (!dynamicsRobot)
394  {
395  ARMARX_WARNING_S << "No robot available with name " << robotName;
396  return;
397  }
398 
399  RobotPtr kinRob = dynamicsRobot->getRobot();
400 
401  if (!kinRob || kinRob->getName() != robotName)
402  {
403  ARMARX_WARNING_S << "No robot available with name " << robotName;
404  return;
405  }
406 
407  RobotNodePtr rn = kinRob->getRobotNode(robotNodeName);
408 
409  if (!rn)
410  {
411  ARMARX_WARNING_S << "No robotnode available with name " << robotNodeName;
412  return;
413  }
414 
415  dynamicsRobot->getDynamicsRobotNode(rn)->applyTorque(torque);
416 }
417 
418 void BulletPhysicsWorld::applyForceObject(const std::string& objectName, const Eigen::Vector3f& force)
419 {
420  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
421  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
422 
423  if (!o)
424  {
425  ARMARX_WARNING_S << "No object found with name " << objectName;
426  return;
427  }
428 
429  o->applyForce(force);
430 }
431 
432 void BulletPhysicsWorld::applyTorqueObject(const std::string& objectName, const Eigen::Vector3f& torque)
433 {
434  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
435  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
436 
437  if (!o)
438  {
439  ARMARX_WARNING_S << "No object found with name " << objectName;
440  return;
441  }
442 
443  o->applyTorque(torque);
444 }
445 
446 
447 SimDynamics::DynamicsObjectPtr BulletPhysicsWorld::getObject(const std::string& objectName)
448 {
449  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
450  SimDynamics::DynamicsObjectPtr o;
451  std::vector<SimDynamics::DynamicsObjectPtr>::const_iterator it;
452 
453  for (it = dynamicsObjects.begin(); it != dynamicsObjects.end(); it++)
454  {
455  if ((*it)->getName() == objectName)
456  {
457  o = *it;
458  break;
459  }
460  }
461 
462  return o;
463 }
464 
465 bool BulletPhysicsWorld::hasObject(const std::string& instanceName)
466 {
467  return getObject(instanceName).get();
468 }
469 
470 void BulletPhysicsWorld::setObjectPose(const std::string& objectName, const Eigen::Matrix4f& globalPose)
471 {
472  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
473  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
474 
475  if (!o)
476  {
477  ARMARX_WARNING_S << "No object found with name " << objectName;
478  return;
479  }
480 
481  o->setPose(globalPose);
482 }
483 
484 void BulletPhysicsWorld::activateObject(const std::string& objectName)
485 {
486  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
487  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
488 
489  if (!o)
490  {
491  ARMARX_WARNING_S << "No object found with name " << objectName;
492  return;
493  }
494 
495  // need newest simox version for this (2.3.48)
496  //o->activate();
497 
498  // this works for earlier versions but activates all objects
499  bulletEngine->activateAllObjects(); // avoid sleeping objects
500 }
501 
502 
503 float BulletPhysicsWorld::getRobotMass(const std::string& robotName)
504 {
505  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
506 
507  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
508  if (!dynamicsRobot)
509  {
510  ARMARX_WARNING_S << "No robot available";
511  return 0;
512  }
513 
514  RobotPtr kinRob = dynamicsRobot->getRobot();
515 
516  if (!kinRob || kinRob->getName() != robotName)
517  {
518  ARMARX_WARNING_S << "No robot available with name " << robotName;
519  return 0;
520  }
521 
522  return kinRob->getMass();
523 }
524 
525 std::map< std::string, float> BulletPhysicsWorld::getRobotJointAngles(const std::string& robotName)
526 {
527  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
528  std::map< std::string, float> res;
529 
530  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
531  if (!dynamicsRobot)
532  {
533  ARMARX_WARNING_S << "No robot available";
534  return res;
535  }
536 
537  RobotPtr kinRob = dynamicsRobot->getRobot();
538 
539  if (!kinRob || kinRob->getName() != robotName)
540  {
541  ARMARX_WARNING_S << "No robot available with name " << robotName;
542  return res;
543  }
544 
545  std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
546 
547  for (auto& i : rn)
548  {
549  if (i->isJoint())
550  {
551  res[i->getName()] = i->getJointValue();
552  }
553  }
554 
555  ARMARX_DEBUG_S << "getRobotJointAngles:" << res.begin()->second;
556 
557  return res;
558 }
559 
560 float BulletPhysicsWorld::getRobotJointAngle(const std::string& robotName, const std::string& nodeName)
561 {
562  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
563 
564  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
565  if (!dynamicsRobot)
566  {
567  ARMARX_WARNING_S << "No robot available";
568  return 0;
569  }
570 
571  RobotPtr kinRob = dynamicsRobot->getRobot();
572 
573  if (!kinRob || kinRob->getName() != robotName)
574  {
575  ARMARX_WARNING_S << "No robot available with name " << robotName;
576  return 0;
577  }
578 
579  if (!kinRob->hasRobotNode(nodeName))
580  {
581  ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
582  return 0;
583  }
584 
585  float res = kinRob->getRobotNode(nodeName)->getJointValue();
586  ARMARX_DEBUG_S << "getRobotJointAngle:" << res;
587  return res;
588 }
589 
590 std::map< std::string, float> BulletPhysicsWorld::getRobotJointVelocities(const std::string& robotName)
591 {
592  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
593  std::map< std::string, float> res;
594 
595  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
596  if (!dynamicsRobot)
597  {
598  ARMARX_WARNING_S << "No robot available";
599  return res;
600  }
601 
602  RobotPtr kinRob = dynamicsRobot->getRobot();
603 
604  if (!kinRob || kinRob->getName() != robotName)
605  {
606  ARMARX_WARNING_S << "No robot available with name " << robotName;
607  return res;
608  }
609 
610  std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
611 
612  for (auto& i : rn)
613  {
614  if (i->isJoint())
615  {
616  float vel = dynamicsRobot->getJointSpeed(i);
617  res[i->getName()] = vel;
618  }
619  }
620 
621  ARMARX_DEBUG_S << "getRobotJointVelocities:" << res.begin()->second;
622  return res;
623 }
624 
625 float BulletPhysicsWorld::getRobotJointVelocity(const std::string& robotName, const std::string& nodeName)
626 {
627  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
628 
629  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
630  if (!dynamicsRobot)
631  {
632  ARMARX_WARNING_S << "No robot available";
633  return 0;
634  }
635 
636  RobotPtr kinRob = dynamicsRobot->getRobot();
637 
638  if (!kinRob || kinRob->getName() != robotName)
639  {
640  ARMARX_WARNING_S << "No robot available with name " << robotName;
641  return 0;
642  }
643 
644  if (!kinRob->hasRobotNode(nodeName))
645  {
646  ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
647  return 0;
648  }
649 
650  double res = dynamicsRobot->getJointSpeed(kinRob->getRobotNode(nodeName));
651  ARMARX_DEBUG_S << "getRobotJointVelocity:" << res;
652  return static_cast<float>(res);
653 }
654 
655 std::map<std::string, float> BulletPhysicsWorld::getRobotJointTorques(const std::string& robotName)
656 {
657  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
658  std::map< std::string, float> res;
659 
660  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
661  if (!dynamicsRobot)
662  {
663  ARMARX_WARNING_S << "No robot available";
664  return res;
665  }
666 
667  VirtualRobot::RobotPtr kinRob = dynamicsRobot->getRobot();
668 
669  if (!kinRob || kinRob->getName() != robotName)
670  {
671  ARMARX_WARNING_S << "No robot available with name " << robotName;
672  return res;
673  }
674 
675  SimDynamics::BulletRobot* br = dynamic_cast<BulletRobot*>(dynamicsRobot.get());
676  if (!br)
677  {
678  ARMARX_WARNING_S << "No bullet robot available with name " << robotName;
679  return res;
680  }
681  std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
682 
683  for (auto& i : rn)
684  {
685  if (i->isJoint() && br)
686  {
687  if (dynamicsRobot->isNodeActuated(i))
688  {
689  res[i->getName()] = br->getJointTorque(i);
690  }
691  }
692  }
693  return res;
694 }
695 
696 ForceTorqueDataSeq BulletPhysicsWorld::getRobotForceTorqueSensors(const std::string& robotName)
697 {
698  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
699  DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
700  RobotPtr virtualRobot = getRobot(robotName);
701  if (!dynamicsRobot || !virtualRobot)
702  {
703  static bool printed = false;
704  if (!printed)
705  {
706  ARMARX_WARNING_S << "No robot available with name " << robotName;
707  printed = true;
708  }
709  }
710  const auto ftsensors = virtualRobot->getSensors<ForceTorqueSensor>();
711  ForceTorqueDataSeq result;
712  result.reserve(ftsensors.size());
713  for (const VirtualRobot::ForceTorqueSensorPtr& sensor : ftsensors)
714  {
715  ForceTorqueData data;
716  Eigen::Vector3f force = sensor->getForce();
717  Eigen::Vector3f torque = sensor->getTorque();
718  data.nodeName = sensor->getRobotNode()->getName();
719  data.sensorName = sensor->getName();
720 
721  if (!robotName.empty() && !data.nodeName.empty())
722  {
723  RobotNodePtr rn = dynamicsRobot->getRobot()->getRobotNode(data.nodeName);
724  Eigen::Vector3f forceGlobal = force;
725  Eigen::Vector3f torqueGlobal = torque;
726  if (rn)
727  {
728  Eigen::Matrix3f rotMat = rn->getGlobalPose().block(0, 0, 3, 3);
729  force = rotMat.inverse() * forceGlobal;
730  torque = rotMat.inverse() * torqueGlobal;
731  }
732  }
733  data.force = new Vector3 {force};
734  data.torque = new Vector3 {torque};
735 
736  result.emplace_back(std::move(data));
737  }
738  return result;
739 }
740 
741 float BulletPhysicsWorld::getRobotJointLimitLo(const std::string& robotName, const std::string& nodeName)
742 {
743  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
744 
745  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
746  if (!dynamicsRobot)
747  {
748  ARMARX_WARNING_S << "No robot available";
749  return 0;
750  }
751 
752  RobotPtr kinRob = dynamicsRobot->getRobot();
753 
754  if (!kinRob || kinRob->getName() != robotName)
755  {
756  ARMARX_WARNING_S << "No robot available with name " << robotName;
757  return 0;
758  }
759 
760  if (!kinRob->hasRobotNode(nodeName))
761  {
762  ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
763  return 0;
764  }
765 
766  return kinRob->getRobotNode(nodeName)->getJointLimitLo();
767 }
768 
769 float BulletPhysicsWorld::getRobotJointLimitHi(const std::string& robotName, const std::string& nodeName)
770 {
771  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
772 
773  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
774  if (!dynamicsRobot)
775  {
776  ARMARX_WARNING_S << "No robot available";
777  return 0;
778  }
779 
780  RobotPtr kinRob = dynamicsRobot->getRobot();
781 
782  if (!kinRob || kinRob->getName() != robotName)
783  {
784  ARMARX_WARNING_S << "No robot available with name " << robotName;
785  return 0;
786  }
787 
788  if (!kinRob->hasRobotNode(nodeName))
789  {
790  ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
791  return 0;
792  }
793 
794  return kinRob->getRobotNode(nodeName)->getJointLimitHi();
795 }
796 
797 Eigen::Matrix4f BulletPhysicsWorld::getRobotPose(const std::string& robotName)
798 {
799  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
800  ARMARX_DEBUG_S << "get robot pose";
801 
802  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
803  if (!dynamicsRobot)
804  {
805  ARMARX_WARNING_S << "No robot available";
806  return Eigen::Matrix4f::Identity();
807  }
808 
809  RobotPtr kinRob = dynamicsRobot->getRobot();
810 
811  if (!kinRob || kinRob->getName() != robotName)
812  {
813  ARMARX_WARNING_S << "No robot available with name " << robotName;
814 
815  if (kinRob)
816  {
817  ARMARX_WARNING_S << "available robot:" << kinRob->getName();
818  }
819 
820  return Eigen::Matrix4f::Identity();
821  }
822 
823  return kinRob->getGlobalPose();
824 }
825 
826 float BulletPhysicsWorld::getRobotMaxTorque(const std::string& robotName, const std::string& nodeName)
827 {
828  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
829  ARMARX_DEBUG_S << "getRobotMaxTorque";
830 
831  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
832  if (!dynamicsRobot)
833  {
834  ARMARX_WARNING_S << "Robot '" << robotName << "' does not exist";
835  return 0.0f;
836  }
837 
838  RobotPtr kinRob = dynamicsRobot->getRobot();
839 
840  if (!kinRob || kinRob->getName() != robotName)
841  {
842  ARMARX_WARNING_S << "No robot available with name " << robotName;
843 
844  if (kinRob)
845  {
846  ARMARX_WARNING_S << "available robot:" << kinRob->getName();
847  }
848 
849  return 0.0f;
850  }
851 
852  if (!kinRob->hasRobotNode(nodeName))
853  {
854  ARMARX_WARNING_S << "Robot node '" << nodeName << "' does not exist";
855  return 0.0f;
856  }
857 
858  return kinRob->getRobotNode(nodeName)->getMaxTorque();
859 }
860 
861 void BulletPhysicsWorld::setRobotMaxTorque(const std::string& robotName, const std::string& nodeName, float maxTorque)
862 {
863  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
864  ARMARX_DEBUG_S << "setRobotMaxTorque";
865 
866  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
867  if (!dynamicsRobot)
868  {
869  ARMARX_WARNING_S << "Robot '" << robotName << "' does not exist";
870  return;
871  }
872 
873  RobotPtr kinRob = dynamicsRobot->getRobot();
874 
875  if (!kinRob || kinRob->getName() != robotName)
876  {
877  ARMARX_WARNING_S << "No robot available with name " << robotName;
878 
879  if (kinRob)
880  {
881  ARMARX_WARNING_S << "available robot:" << kinRob->getName();
882  }
883 
884  return;
885  }
886 
887  if (!kinRob->hasRobotNode(nodeName))
888  {
889  ARMARX_WARNING_S << "Robot node '" << nodeName << "' does not exist";
890  return;
891  }
892 
893  kinRob->getRobotNode(nodeName)->setMaxTorque(maxTorque);
894 }
895 
896 Eigen::Matrix4f BulletPhysicsWorld::getRobotNodePose(const std::string& robotName, const std::string& robotNodeName)
897 {
898  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
899  ARMARX_DEBUG_S << "get robot node pose";
900 
901  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
902  if (!dynamicsRobot)
903  {
904  ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
905  return Eigen::Matrix4f::Identity();
906  }
907 
908  RobotPtr kinRob = dynamicsRobot->getRobot();
909 
910  if (!kinRob || kinRob->getName() != robotName)
911  {
912  ARMARX_WARNING_S << "No robot available with name " << robotName;
913  return Eigen::Matrix4f::Identity();
914  }
915 
916  if (!kinRob->hasRobotNode(robotNodeName))
917  {
918  ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
919  return Eigen::Matrix4f::Identity();
920  }
921 
922  return kinRob->getRobotNode(robotNodeName)->getGlobalPose();
923 }
924 
925 
926 Eigen::Vector3f BulletPhysicsWorld::getRobotLinearVelocity(const std::string& robotName, const std::string& nodeName)
927 {
928  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
929  ARMARX_DEBUG_S << "get robot linear velocity";
930 
931  DynamicsObjectPtr dynamicsRobotNode = this->getFirstDynamicsObject(robotName, nodeName);
932 
933  if (!dynamicsRobotNode)
934  {
935  return Eigen::Vector3f::Identity();
936  }
937  return dynamicsRobotNode->getLinearVelocity();
938 }
939 
940 
941 SimDynamics::DynamicsObjectPtr BulletPhysicsWorld::getFirstDynamicsObject(const std::string& robotName, const std::string& nodeName)
942 {
943  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
944  if (!dynamicsRobot)
945  {
946  Ice::StringSeq robots;
947  for (auto& elem : getRobots())
948  {
949  robots.push_back(elem.first);
950  }
951 
952  ARMARX_WARNING_S << deactivateSpam(1) << "No robot available with name '" << robotName << "' - available robots: " << robots;
953 
954  return {};
955 
956  }
957 
958  RobotPtr kinRob = dynamicsRobot->getRobot();
959 
960  if (!kinRob || kinRob->getName() != robotName)
961  {
962  ARMARX_WARNING_S << "No robot available with name " << robotName;
963  return {};
964  }
965 
966  if (!kinRob->hasRobotNode(nodeName))
967  {
968  ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
969  return {};
970  }
971 
972 
973  RobotNodePtr robotNode = kinRob->getRobotNode(nodeName);
974  DynamicsObjectPtr dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
975  // search first parent with dynamic object
976 
977  while (!dynamicsRobotNode)
978  {
979  robotNode = std::dynamic_pointer_cast<RobotNode>(robotNode->getParent());
980  if (!robotNode)
981  {
982  ARMARX_WARNING_S << "unable to get parent robot node";
983  return {};
984 
985  }
986  dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
987  }
988 
989  return dynamicsRobotNode;
990 }
991 
992 
993 Eigen::Vector3f BulletPhysicsWorld::getRobotAngularVelocity(const std::string& robotName, const std::string& nodeName)
994 {
995  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
996  ARMARX_DEBUG_S << "get robot angular vel";
997 
998  DynamicsObjectPtr dynamicsRobotNode = this->getFirstDynamicsObject(robotName, nodeName);
999 
1000  if (!dynamicsRobotNode)
1001  {
1002  return Eigen::Vector3f::Identity();
1003  }
1004 
1005  return dynamicsRobotNode->getAngularVelocity();
1006 }
1007 
1008 void BulletPhysicsWorld::setRobotLinearVelocity(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
1009 {
1010  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1011  ARMARX_DEBUG_S << "set robot lin vel";
1012 
1013  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1014  if (!dynamicsRobot)
1015  {
1016  ARMARX_WARNING_S << "No robot available";
1017  return;
1018  }
1019 
1020  RobotPtr kinRob = dynamicsRobot->getRobot();
1021 
1022  if (!kinRob || kinRob->getName() != robotName)
1023  {
1024  ARMARX_WARNING_S << "No robot available with name " << robotName;
1025  return;
1026  }
1027 
1028  if (!kinRob->hasRobotNode(robotNodeName))
1029  {
1030  ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
1031  return;
1032  }
1033 
1034  dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setLinearVelocity(vel);
1035  ARMARX_DEBUG_S << "set robot lin vel end";
1036 }
1037 
1038 void BulletPhysicsWorld::setRobotAngularVelocity(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
1039 {
1040  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1041  ARMARX_DEBUG_S << "set robot ang vel";
1042 
1043  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1044  if (!dynamicsRobot)
1045  {
1046  ARMARX_WARNING_S << "No robot available";
1047  return;
1048  }
1049 
1050  RobotPtr kinRob = dynamicsRobot->getRobot();
1051 
1052  if (!kinRob || kinRob->getName() != robotName)
1053  {
1054  ARMARX_WARNING_S << "No robot available with name " << robotName;
1055  return;
1056  }
1057 
1058  if (!kinRob->hasRobotNode(robotNodeName))
1059  {
1060  ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
1061  return;
1062  }
1063 
1064  dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setAngularVelocity(vel);
1065  ARMARX_DEBUG_S << "set robot ang vel end";
1066 }
1067 
1068 void BulletPhysicsWorld::setRobotLinearVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
1069 {
1070  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1071  ARMARX_DEBUG_S << "set robot lin vel";
1072 
1073  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1074  if (!dynamicsRobot)
1075  {
1076  ARMARX_WARNING_S << "No robot available";
1077  return;
1078  }
1079 
1080  RobotPtr kinRob = dynamicsRobot->getRobot();
1081 
1082  if (!kinRob || kinRob->getName() != robotName)
1083  {
1084  ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
1085  return;
1086  }
1087 
1088  if (!kinRob->hasRobotNode(robotNodeName))
1089  {
1090  ARMARX_WARNING_S << "No robotNode available with name '" << robotNodeName << "'";
1091  return;
1092  }
1093 
1094  if (!dynamicsRobot->hasDynamicsRobotNode(kinRob->getRobotNode(robotNodeName)))
1095  {
1096  ARMARX_WARNING_S << "No dynamic robotNode available with name '" << robotNodeName << "'";
1097  return;
1098  }
1099 
1100  Eigen::Matrix4f newPose;
1101  newPose.setIdentity();
1102  newPose.block<3, 1>(0, 3) = vel;
1103  auto globalPose = dynamicsRobot->getRobot()->getGlobalPose();
1104  globalPose(0, 3) = 0;
1105  globalPose(1, 3) = 0;
1106  globalPose(2, 3) = 0;
1107  auto globalNewPose = globalPose * newPose;
1108  dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setLinearVelocity(globalNewPose.block<3, 1>(0, 3));
1109  ARMARX_DEBUG_S << "set robot lin vel end";
1110 }
1111 
1112 void BulletPhysicsWorld::setRobotAngularVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
1113 {
1114  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1115  ARMARX_DEBUG_S << "set robot ang vel";
1116 
1117  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1118  if (!dynamicsRobot)
1119  {
1120  ARMARX_WARNING_S << "No robot available";
1121  return;
1122  }
1123 
1124  RobotPtr kinRob = dynamicsRobot->getRobot();
1125 
1126  if (!kinRob || kinRob->getName() != robotName)
1127  {
1128  ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
1129  return;
1130  }
1131 
1132  if (!kinRob->hasRobotNode(robotNodeName))
1133  {
1134  ARMARX_WARNING_S << "No robotNode available with name '" << robotNodeName << "'";
1135  return;
1136  }
1137 
1138  if (!dynamicsRobot->hasDynamicsRobotNode(kinRob->getRobotNode(robotNodeName)))
1139  {
1140  ARMARX_WARNING_S << "No dynamic robotNode available with name '" << robotNodeName << "'";
1141  return;
1142  }
1143 
1144  Eigen::Matrix4f newPose;
1145  newPose.setIdentity();
1146  newPose.block<3, 1>(0, 3) = vel;
1147  auto globalPose = dynamicsRobot->getRobot()->getGlobalPose();
1148  globalPose(0, 3) = 0;
1149  globalPose(1, 3) = 0;
1150  globalPose(2, 3) = 0;
1151  auto globalNewPose = globalPose * newPose;
1152 
1153  RobotNodePtr robotNode = kinRob->getRobotNode(robotNodeName);
1154  DynamicsObjectPtr dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
1155  dynamicsRobotNode->setAngularVelocity(globalNewPose.block<3, 1>(0, 3));
1156 }
1157 
1158 bool BulletPhysicsWorld::hasRobot(const std::string& robotName)
1159 {
1160  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1161 
1162  std::map<std::string, DynamicsRobotInfo>::iterator it = dynamicRobots.begin();
1163  while (it != dynamicRobots.end())
1164  {
1165  if (it->first == robotName)
1166  {
1167  return true;
1168  }
1169  it++;
1170  }
1171 
1172  return false;
1173 }
1174 
1175 
1176 bool BulletPhysicsWorld::hasRobotNode(const std::string& robotName, const std::string& robotNodeName)
1177 {
1178  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1179 
1180  SimDynamics::DynamicsRobotPtr dynRob = getDynamicRobot(robotName);
1181  if (!dynRob)
1182  {
1183  return false;
1184  }
1185 
1186  return (dynRob->getRobot()->hasRobotNode(robotNodeName));
1187 }
1188 
1189 
1190 VirtualRobot::RobotPtr BulletPhysicsWorld::getRobot(const std::string& robotName)
1191 {
1192  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1193  SimDynamics::DynamicsRobotPtr r = getDynamicRobot(robotName);
1194  if (!r)
1195  {
1196  return VirtualRobot::RobotPtr();
1197  }
1198 
1199  return r->getRobot();
1200 }
1201 
1202 std::map<std::string, RobotPtr> BulletPhysicsWorld::getRobots()
1203 {
1204  std::map<std::string, RobotPtr> result;
1205  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1206  for (auto& elem : dynamicRobots)
1207  {
1208  const DynamicsRobotInfo& info = elem.second;
1209  result[elem.first] = (info.robot);
1210  }
1211  return result;
1212 }
1213 
1214 SimDynamics::DynamicsRobotPtr BulletPhysicsWorld::getDynamicRobot(const std::string& robotName)
1215 {
1216  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1217 
1218  if (!hasRobot(robotName))
1219  {
1220  return SimDynamics::DynamicsRobotPtr();
1221  }
1222 
1223  return dynamicRobots[robotName].dynamicsRobot;
1224 }
1225 
1226 SimDynamics::DynamicsObjectPtr BulletPhysicsWorld::getDynamicsObject(const std::string& objectName)
1227 {
1228  SimDynamics::DynamicsObjectPtr o;
1229  std::vector<SimDynamics::DynamicsObjectPtr>::const_iterator it;
1230 
1231  for (it = dynamicsObjects.begin(); it != dynamicsObjects.end(); it++)
1232  {
1233  if ((*it)->getName() == objectName)
1234  {
1235  o = *it;
1236  break;
1237  }
1238  }
1239 
1240  return o;
1241 }
1242 
1243 Eigen::Matrix4f BulletPhysicsWorld::getObjectPose(const std::string& objectName)
1244 {
1245  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1246  ARMARX_DEBUG_S << "get object pose";
1247  SimDynamics::DynamicsObjectPtr o = getDynamicsObject(objectName);
1248 
1249  if (!o)
1250  {
1251  ARMARX_WARNING_S << "No object found with name " << objectName;
1252  return Eigen::Matrix4f::Identity();
1253  }
1254 
1255  return o->getSceneObject()->getGlobalPose();
1256 }
1257 
1258 bool BulletPhysicsWorld::objectReleasedEngine(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName)
1259 {
1260  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1261  ARMARX_INFO_S << "Object released, robot " << robotName << ", node:" << robotNodeName << ", object:" << objectName;
1262  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1263  if (!dynamicsRobot)
1264  {
1265  ARMARX_WARNING_S << "No robot available with name " << robotName;
1266  return false;
1267  }
1268  RobotPtr kinRob = dynamicsRobot->getRobot();
1269 
1270  if (!kinRob || kinRob->getName() != robotName)
1271  {
1272  ARMARX_WARNING_S << "No robot available with name " << robotName;
1273  return false;
1274  }
1275 
1276  if (!kinRob->hasRobotNode(robotNodeName))
1277  {
1278  ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
1279  return false;
1280  }
1281 
1282  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
1283 
1284  if (!o)
1285  {
1286  ARMARX_WARNING_S << "No object found with name " << objectName;
1287  return false;
1288  }
1289 
1290  bool res = dynamicsWorld->getEngine()->detachObjectFromRobot(robotName, o);
1291 
1292  return res;
1293 }
1294 
1295 bool BulletPhysicsWorld::objectGraspedEngine(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName, Eigen::Matrix4f& storeLocalTransform)
1296 {
1297  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1298  ARMARX_INFO_S << "Object grasped, robot " << robotName << ", node:" << robotNodeName << ", object:" << objectName;
1299 
1300  // check if object is already grasped
1301  for (const auto& ao : attachedObjects)
1302  {
1303  if (ao.robotName == robotName && ao.robotNodeName == robotNodeName && ao.objectName == objectName)
1304  {
1305  ARMARX_INFO_S << "Object already attached, skipping...";
1306  return false;
1307  }
1308  }
1309  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1310  RobotPtr kinRob = dynamicsRobot->getRobot();
1311 
1312  if (!kinRob || kinRob->getName() != robotName)
1313  {
1314  ARMARX_WARNING_S << "No robot available with name " << robotName;
1315  return false;
1316  }
1317 
1318  if (!kinRob->hasRobotNode(robotNodeName))
1319  {
1320  ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
1321  return false;
1322  }
1323 
1324  RobotNodePtr rn = kinRob->getRobotNode(robotNodeName);
1325 
1326 
1327  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
1328 
1329  if (!o)
1330  {
1331  ARMARX_WARNING_S << "No object found with name " << objectName;
1332  return false;
1333  }
1334 
1335  bool res = dynamicsWorld->getEngine()->attachObjectToRobot(robotName, robotNodeName, o);
1336  storeLocalTransform = rn->toLocalCoordinateSystem(o->getSceneObject()->getGlobalPose());
1337  return res;
1338 }
1339 
1340 
1341 SceneObjectPtr BulletPhysicsWorld::getFloor()
1342 {
1343  if (!dynamicsWorld || !dynamicsWorld->getEngine()->getFloor())
1344  {
1345  return SceneObjectPtr();
1346  }
1347  return dynamicsWorld->getEngine()->getFloor()->getSceneObject();
1348 }
1349 
1350 
1351 
1352 bool BulletPhysicsWorld::removeRobotEngine(const std::string& robotName)
1353 {
1354  // lock engine and data
1355  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1356 
1357  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1358  if (!dynamicsWorld || !dynamicsRobot)
1359  {
1360  return false;
1361  }
1362 
1363  ARMARX_DEBUG_S << "Removing robot " << robotName;
1364 
1365 
1366  //access engine
1367  {
1368  if (robotName != dynamicsRobot->getRobot()->getName())
1369  {
1370  ARMARX_WARNING_S << "Robot names differ...";
1371  }
1372 
1373  dynamicsWorld->removeRobot(dynamicsRobot);
1374  dynamicRobots.erase(robotName);
1375  }
1376 
1377  return true;
1378 }
1379 
1380 
1381 bool BulletPhysicsWorld::addRobotEngine(RobotPtr robot, double pid_p, double pid_i, double pid_d, const std::string& filename, bool staticRobot, bool selfCollisions)
1382 {
1383  ARMARX_VERBOSE_S << "Adding robot " << robot->getName() << ", file: " << filename;
1384 
1385  // lock engine and data
1386  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1387 
1388  //access engine
1389  ARMARX_DEBUG_S << "add robot";
1390 
1391  if (!dynamicsWorld)
1392  {
1393  ARMARX_ERROR_S << "No physics world!";
1394  return false;
1395  }
1396 
1397  if (hasRobot(robot->getName()))
1398  {
1399  ARMARX_ERROR_S << "More than one robot with identical name is currently not supported!";
1400  return false;
1401  }
1402 
1403  DynamicsRobotInfo robInfo;
1404  if (staticRobot)
1405  {
1406  ARMARX_INFO_S << "Building static robot...";
1407  std::vector< RobotNodePtr > nodes = robot->getRobotNodes();
1408  for (auto n : nodes)
1409  {
1410  n->setSimulationType(SceneObject::Physics::eKinematic);
1411  }
1412  }
1413 
1414  SimDynamics::DynamicsRobotPtr dynamicsRobot;
1415  try
1416  {
1417  dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
1418  ARMARX_DEBUG_S << "add dynamicsRobot" << dynamicsRobot->getName();
1419 
1420  auto& controllers = dynamicsRobot->getControllers();
1421  std::vector<RobotNodePtr> nodes = robot->getRobotNodes();
1422 
1423  if (not robot->isPassive())
1424  {
1425  ARMARX_INFO_S << "Configuring controllers for robot " << dynamicsRobot->getName();
1426 
1427  for (auto node : nodes)
1428  {
1429  if (node->isRotationalJoint())
1430  {
1431  if (controllers.find(node) == controllers.end())
1432  {
1433  controllers[node] = VelocityMotorController(
1434  static_cast<double>(node->getMaxVelocity()),
1435  static_cast<double>(node->getMaxAcceleration()));
1436  }
1437  controllers[node].reset(pid_p, pid_i, pid_d);
1438  }
1439  }
1440  }
1441 
1442  ARMARX_INFO_S << "The robot " << dynamicsRobot->getName() << " has " << controllers.size() << " controllers";
1443 
1444  if (not selfCollisions)
1445  {
1446  dynamicsRobot->enableSelfCollisions(false); // do not pass the value selfCollisions in here, because it would reset the specifically set ingored-collisions.
1447  }
1448 
1449  dynamicsWorld->addRobot(dynamicsRobot);
1450  ARMARX_DEBUG_S << "add dynamicsRobot2 " << dynamicsRobot->getName();
1451  }
1452  catch (VirtualRobotException& e)
1453  {
1454  ARMARX_ERROR_S << " ERROR while building dynamic robot";
1455  ARMARX_ERROR_S << e.what();
1456  return false;
1457  }
1458 
1459 
1460 
1461  // since we do not visualize this robot we can skip visualization updates
1462  dynamicsRobot->getRobot()->setThreadsafe(false); // we already got a mutex protection
1463  dynamicsRobot->getRobot()->setUpdateVisualization(false);
1464 
1465 
1466  robInfo.isStatic = staticRobot;
1467  robInfo.robot = robot;
1468  robInfo.dynamicsRobot = dynamicsRobot;
1469 
1470  dynamicRobots[robot->getName()] = robInfo;
1471 
1472  return true;
1473 }
1474 
1475 armarx::FramedPosePtr BulletPhysicsWorld::toFramedPose(const Eigen::Matrix4f& globalPose, const std::string& robotName, const std::string& frameName)
1476 {
1477  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1478 
1479  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1480 
1481  if (!dynamicsRobot || dynamicsRobot->getRobot()->getName() != robotName)
1482  {
1483  ARMARX_WARNING_S << "No robot or wrong name: " << robotName;
1484  return FramedPosePtr();
1485  }
1486 
1487  RobotNodePtr rn = dynamicsRobot->getRobot()->getRobotNode(frameName);
1488 
1489  if (!rn)
1490  {
1491  ARMARX_WARNING_S << "No robot node found(" << frameName << ")";
1492  return FramedPosePtr();
1493  }
1494 
1495  Eigen::Matrix4f localPose = rn->toLocalCoordinateSystem(globalPose);
1496 
1497  armarx::FramedPosePtr framedPose = new armarx::FramedPose(localPose, frameName, dynamicsRobot->getRobot()->getName());
1498  return framedPose;
1499 }
1500 
1501 bool BulletPhysicsWorld::synchronizeRobotNodePoses(const std::string& robotName, std::map<std::string, armarx::PoseBasePtr>& objMap)
1502 {
1503  // lock engine and data
1504  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1505  ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
1506 
1507  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1508  // update visu data
1509  if (!dynamicsRobot || robotName != dynamicsRobot->getName())
1510  {
1511  //ARMARX_ERROR_S << "no robot data";
1512  return false;
1513  }
1514 
1515  VirtualRobot::RobotPtr kinRobEngine = dynamicsRobot->getRobot();
1516  VirtualRobot::SceneObjectPtr currentObjEngine = kinRobEngine->getRootNode();
1517 
1518  if (!synchronizeSceneObjectPoses(currentObjEngine, objMap))
1519  {
1520  ARMARX_ERROR_S << "Failed to synchronize objects...";
1521  return false;
1522  }
1523  return true;
1524 }
1525 
1526 bool BulletPhysicsWorld::getRobotStatus(const std::string& robotName, NameValueMap& jointAngles, NameValueMap& jointVelocities, NameValueMap& jointTorques, Eigen::Vector3f& linearVelocity, Eigen::Vector3f& angularVelocity)
1527 {
1528  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1529  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1530 
1531  if (!dynamicsRobot || robotName != dynamicsRobot->getName())
1532  {
1533  return false;
1534  }
1535 
1536  double simTimeFactor = 1.0;
1537  if (simStepExecutionDurationMS > 0)
1538  {
1539  simTimeFactor = static_cast<double>(simTimeStepMS / simStepExecutionDurationMS);
1540  }
1541 
1542  VirtualRobot::RobotPtr kinRob = dynamicsRobot->getRobot();
1543 
1544  SimDynamics::BulletRobotPtr br = std::dynamic_pointer_cast<BulletRobot>(dynamicsRobot);
1545  std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
1546 
1547  for (auto& i : rn)
1548  {
1549  if (i->isJoint())
1550  {
1551  jointAngles[i->getName()] = i->getJointValue();
1552 
1553  // TODO: check if isNodeActuated() is needed as well as else clause.
1554  // jointSpeed should also be valid / meaningful for a passive robot
1555  if (dynamicsRobot->isNodeActuated(i))
1556  {
1557  jointVelocities[i->getName()] = dynamicsRobot->getJointSpeed(i) * simTimeFactor;
1558 
1559  if (br)
1560  {
1561  jointTorques[i->getName()] = br->getJointTorque(i);
1562  }
1563  }
1564  else
1565  {
1566  jointVelocities[i->getName()] = 0;
1567  jointTorques[i->getName()] = 0;
1568  }
1569  }
1570  }
1571 
1572  DynamicsObjectPtr rootDyn = dynamicsRobot->getDynamicsRobotNode(kinRob->getRootNode());
1573 
1574  if (rootDyn)
1575  {
1576  linearVelocity = rootDyn->getLinearVelocity();
1577  angularVelocity = rootDyn->getAngularVelocity();
1578  }
1579  else
1580  {
1581  linearVelocity.setZero();
1582  angularVelocity.setZero();
1583  }
1584 
1585  return true;
1586 }
1587 
1588 bool BulletPhysicsWorld::updateForceTorqueSensor(ForceTorqueInfo& ftInfo)
1589 {
1590  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1591  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(ftInfo.robotName);
1592  if (!dynamicsRobot)
1593  {
1594  static bool printed = false;
1595  if (!printed)
1596  {
1597  ARMARX_WARNING_S << "No robot available with name " << ftInfo.robotName;
1598  printed = true;
1599  }
1600 
1601  return false;
1602  }
1603 
1604  // update force torque info
1605  //for (size_t i = 0; i < simReportData.robots.at(0).forceTorqueSensors.size(); i++)
1606  //{
1607  if (ftInfo.enable)
1608  {
1609  ftInfo.currentForce = ftInfo.ftSensor->getForce();
1610  ftInfo.currentTorque = ftInfo.ftSensor->getTorque();
1611 
1612  // transform to local coordinates
1613  std::string nodeName = ftInfo.robotNodeName;
1614  std::string robotName = ftInfo.robotName;
1615 
1616  if (!robotName.empty() && !nodeName.empty() && dynamicsRobot)
1617  {
1618  RobotNodePtr rn = dynamicsRobot->getRobot()->getRobotNode(nodeName);
1619  Eigen::Vector3f forceLocal;
1620  Eigen::Vector3f forceGlobal = ftInfo.currentForce;
1621  Eigen::Vector3f torqueLocal;
1622  Eigen::Vector3f torqueGlobal = ftInfo.currentTorque;
1623 
1624  if (rn)
1625  {
1626  Eigen::Matrix3f rotMat = rn->getGlobalPose().block(0, 0, 3, 3);
1627  forceLocal = rotMat.inverse() * forceGlobal;
1628  torqueLocal = rotMat.inverse() * torqueGlobal;
1629  ftInfo.currentForce = forceLocal;
1630  ftInfo.currentTorque = torqueLocal;
1631  }
1632  }
1633  }
1634  //}
1635 
1636  return true;
1637 }
1638 
1639 
1640 bool BulletPhysicsWorld::synchronizeObjects()
1641 {
1642  // lock engine and data
1643  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1644  ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
1645 
1646  for (auto& dynamicsObject : dynamicsObjects)
1647  {
1648  SceneObjectPtr oEngine = dynamicsObject->getSceneObject();
1649  bool objectFound = false;
1650 
1651  for (auto& s : simVisuData.objects)
1652  {
1653  if (s.name == oEngine->getName())
1654  {
1655  objectFound = true;
1656  synchronizeSceneObjectPoses(oEngine, s.objectPoses);
1657  s.updated = true;
1658  break;
1659  }
1660  }
1661 
1662  if (!objectFound)
1663  {
1664  ARMARX_WARNING_S << "Object with name " << oEngine->getName() << " not in synchronized list";
1665  }
1666  }
1667 
1668  return true;
1669 }
1670 
1671 
1672 std::vector<DynamicsEngine::DynamicsContactInfo> BulletPhysicsWorld::copyContacts()
1673 {
1674  ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
1675  return contacts;
1676 }
1677 
1678 bool BulletPhysicsWorld::addObstacleEngine(
1679  VirtualRobot::SceneObjectPtr o,
1680  VirtualRobot::SceneObject::Physics::SimulationType simType)
1681 {
1682  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1683 
1684  ARMARX_DEBUG_S << "Adding obstacle " << o->getName();
1685  //clonedObj = o->clone(o->getName());
1686 
1687  DynamicsObjectPtr dynamicsObject;
1688 
1689  try
1690  {
1691  if (simType != VirtualRobot::SceneObject::Physics::eUnknown)
1692  {
1693  o->setSimulationType(simType); //clonedObj
1694  }
1695 
1696  dynamicsObject = dynamicsWorld->CreateDynamicsObject(o);//clonedObj
1697  dynamicsWorld->addObject(dynamicsObject);
1698 
1699  }
1700  catch (VirtualRobotException& e)
1701  {
1702  cout << " ERROR while building dynamic object";
1703  cout << e.what();
1704  return false;
1705  }
1706 
1707  dynamicsObjects.push_back(dynamicsObject);
1708 
1709  // since we do not visualize this object we can skip visualization updates
1710  dynamicsObject->getSceneObject()->setUpdateVisualization(false);
1711 
1712  return true;
1713 }
1714 
1715 std::vector<std::string> BulletPhysicsWorld::getRobotNames()
1716 {
1717  std::vector<std::string> names;
1718  {
1719  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1720  std::map<std::string, DynamicsRobotInfo>::iterator it = dynamicRobots.begin();
1721  while (it != dynamicRobots.end())
1722  {
1723  names.push_back(it->first);
1724  it++;
1725  }
1726  }
1727  return names;
1728 }
1729 
1730 void BulletPhysicsWorld::setObjectSimType(const std::string& objectName, SceneObject::Physics::SimulationType simType)
1731 {
1732  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1733  SimDynamics::DynamicsObjectPtr o = getObject(objectName);
1734  if (!o)
1735  {
1736  ARMARX_WARNING_S << "Could not find object with name " << objectName;
1737  return;
1738  }
1739  o->setSimType(simType);
1740 }
1741 
1742 void BulletPhysicsWorld::setRobotNodeSimType(const std::string& robotName, const std::string& robotNodeName, SceneObject::Physics::SimulationType simType)
1743 {
1744  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1745  SimDynamics::DynamicsRobotPtr r = getDynamicRobot(robotName);
1746  if (!r)
1747  {
1748  ARMARX_WARNING_S << "Could not find robot with name " << robotName;
1749  return;
1750  }
1751  DynamicsObjectPtr rn = r->getDynamicsRobotNode(robotNodeName);
1752  if (!rn)
1753  {
1754  ARMARX_WARNING_S << "Could not find robot node with name " << robotNodeName;
1755  return;
1756  }
1757  rn->setSimType(simType);
1758 }
1759 
1760 std::vector<std::string> BulletPhysicsWorld::getObstacleNames()
1761 {
1762  std::vector<std::string> names;
1763  {
1764  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1765 
1766  for (auto& o : dynamicsObjects)
1767  {
1768  names.push_back(o->getName());
1769  }
1770  }
1771  return names;
1772 }
1773 
1774 
1775 bool BulletPhysicsWorld::removeObstacleEngine(const std::string& name)
1776 {
1777  ARMARX_VERBOSE_S << "Removing obstacle " << name;
1778  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1779  {
1780  SimDynamics::DynamicsObjectPtr o = getObject(name);
1781 
1782  if (!o)
1783  {
1784  return false;
1785  }
1786 
1787  std::vector<SimDynamics::DynamicsObjectPtr>::iterator it = find(dynamicsObjects.begin(), dynamicsObjects.end(), o);
1788 
1789  if (it != dynamicsObjects.end())
1790  {
1791  dynamicsObjects.erase(it);
1792  }
1793  else
1794  {
1795  ARMARX_WARNING_S << "Sync error";
1796  }
1797 
1798  dynamicsWorld->removeObject(o);
1799  }
1800 
1801  return true;
1802 }
1803 
1804 
1805 
1806 void BulletPhysicsWorld::stepPhysicsRealTime()
1807 {
1808  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1809  ARMARX_DEBUG_S << "start sim physics";
1810 
1811 
1812  // IceUtil::Time startTime = IceUtil::Time::now();
1813  btScalar dt1 = btScalar(getDeltaTimeMicroseconds() / 1000.0);
1814 
1815  // bulletEngine->activateAllObjects(); // avoid sleeping objects
1816 
1817  // the engine has to be stepped with seconds
1818  // max 100 internal loops
1819  ARMARX_DEBUG << deactivateSpam(1) << "dt1: " << (dt1 / 1000.0) << " max substeps: " << bulletFixedTimeStepMaxNrLoops << " internal step size: " << 0.001 * bulletFixedTimeStepMS;
1820  bulletEngine->stepSimulation(dt1 / 1000.0, bulletFixedTimeStepMaxNrLoops, 0.001 * bulletFixedTimeStepMS);
1821  simTimeStepMS = static_cast<float>(dt1);
1822  // step static robots
1823  stepStaticRobots(dt1 / 1000.0);
1824 
1825 
1826  currentSimTimeSec += dt1 / 1000.0;
1827  // IceUtil::Time duration = IceUtil::Time::now() - startTime;
1828  simStepExecutionDurationMS = static_cast<float>(dt1);
1829  ARMARX_DEBUG_S << "end sim physics, sim duration [ms] : " << simStepExecutionDurationMS;
1830 
1831 }
1832 
1833 
1834 void BulletPhysicsWorld::stepPhysicsFixedTimeStep()
1835 {
1836  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1837  auto startTime = IceUtil::Time::now(); // this should always be real time and not virtual time
1838 
1839  // bulletEngine->activateAllObjects(); // avoid sleeping objects
1840  // bulletFixedTimeStepMaxNrLoops = 1;
1841 
1842  float stepSize = static_cast<float>(stepSizeMs / 1000.0);
1843  ARMARX_DEBUG_S << "start sim physics: " << VAROUT(stepSize) << VAROUT(stepSizeMs);
1844  // int maxSubSteps = 10;
1845  // the engine has to be stepped with seconds
1846  // better results when stepping engine by hand
1847  // for (int i = 0; i < bulletFixedTimeStepMaxNrLoops; i++)
1848  {
1849  bulletEngine->stepSimulation(static_cast<double>(stepSize),
1850  bulletFixedTimeStepMaxNrLoops, 0.001 * bulletFixedTimeStepMS);
1851  }
1852  simTimeStepMS = stepSizeMs;///*bulletFixedTimeStepMaxNrLoops **/ stepSize * 1000.0f;
1853 
1854  // step static robots
1855  stepStaticRobots(/*bulletFixedTimeStepMaxNrLoops **/ static_cast<double>(stepSize));
1856 
1857 
1858  currentSimTimeSec += static_cast<double>(stepSize) /** (float)bulletFixedTimeStepMaxNrLoops*/;
1859 
1860 
1861  if (maxRealTimeSimSpeed > 0)
1862  {
1863  auto duration = (IceUtil::Time::now() - startTime).toMilliSecondsDouble();
1864  const double minStepSizeMs = static_cast<double>(stepSizeMs / maxRealTimeSimSpeed);
1865  if (duration < minStepSizeMs)
1866  {
1867  ARMARX_DEBUG << "Sim calculation took " << duration << " - Sleeping now for " << (stepSizeMs - duration);
1868  usleep(static_cast<unsigned int>(1000 * (minStepSizeMs - duration)));
1869  }
1870  }
1871 
1872  IceUtil::Time duration = IceUtil::Time::now() - startTime;
1873  simStepExecutionDurationMS = static_cast<float>(duration.toMilliSecondsDouble());
1874  ARMARX_DEBUG_S << "end sim physics, sim duration [ms] : " << simStepExecutionDurationMS;
1875 }
1876 
1877 void BulletPhysicsWorld::stepStaticRobots(double deltaInSeconds)
1878 {
1879  // step through all static robots and apply velocities
1880  for (auto dr : dynamicRobots)
1881  {
1882  DynamicsRobotInfo ri = dr.second;
1883  if (ri.isStatic)
1884  {
1885  std::map<std::string, float> targetPos;
1886  VirtualRobot::RobotPtr kinRob = ri.robot;
1887  ARMARX_DEBUG << "Applying velocities for static robot " << kinRob->getName();
1888  for (auto nv : ri.targetVelocities)
1889  {
1890  if (!kinRob->hasRobotNode(nv.first))
1891  {
1892  ARMARX_ERROR << "No rn with name " << kinRob->hasRobotNode(nv.first);
1893  continue;
1894  }
1895  double change = static_cast<double>(nv.second) * deltaInSeconds;
1896 
1897  //double randomNoiseValue = distribution(generator);
1898  //change += randomNoiseValue;
1899  double newAngle = static_cast<double>(kinRob->getRobotNode(nv.first)->getJointValue()) + change;
1900  targetPos[nv.first] = static_cast<float>(newAngle);
1901  }
1902 
1903  if (not targetPos.empty())
1904  {
1905  ARMARX_DEBUG << "Target joint angles : " << targetPos;
1906  actuateRobotJointsPos(kinRob->getName(), targetPos);
1907  }
1908  }
1909  }
1910 }
1911 
1912 int BulletPhysicsWorld::getFixedTimeStepMS()
1913 {
1914  return stepSizeMs;
1915 }
1916 
1917 
1918 SimDynamics::BulletEnginePtr BulletPhysicsWorld::getEngine()
1919 {
1920  return bulletEngine;
1921 }
1922 
1923 
1924 std::vector<VirtualRobot::SceneObjectPtr> BulletPhysicsWorld::getObjects()
1925 {
1926  std::vector<VirtualRobot::SceneObjectPtr> res;
1927  for (auto o : dynamicsObjects)
1928  {
1929  res.push_back(o->getSceneObject());
1930  }
1931  return res;
1932 }
1933 
1934 std::vector<SimDynamics::DynamicsObjectPtr> BulletPhysicsWorld::getDynamicObjects()
1935 {
1936  return dynamicsObjects;
1937 }
1938 
1939 void BulletPhysicsWorld::setupFloorEngine(bool enable, const std::string& floorTexture)
1940 {
1941 
1942  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1943 
1944  if (!dynamicsWorld)
1945  {
1946  return;
1947  }
1948 
1949  std::string textureFile = floorTexture;
1950 
1951  if (enable)
1952  {
1953  ARMARX_INFO_S << "Creating floor plane...";
1954  dynamicsWorld->createFloorPlane();
1955  }
1956 }
1957 
1958 void BulletPhysicsWorld::enableLogging(const std::string& robotName, const std::string& logFile)
1959 {
1960  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
1961  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
1962  BulletRobotPtr br = std::dynamic_pointer_cast<BulletRobot>(dynamicsRobot);
1963 
1964  if (!br)
1965  {
1966  ARMARX_ERROR_S << "no bullet robot to log...";
1967  return;
1968  }
1969 
1970  // create rns with all bodies
1971  std::vector<RobotNodePtr> rnsBodies;
1972  std::vector<RobotNodePtr> rnsJoints;
1973  std::vector<RobotNodePtr> rnAll = br->getRobot()->getRobotNodes();
1974  std::string nameJoints = "RNS_Joints_All_logging";
1975  std::string nameBodies = "RNS_Bodies_All_logging";
1976 
1977  for (auto r : rnAll)
1978  {
1979  if (r->isJoint())
1980  {
1981  rnsJoints.push_back(r);
1982  }
1983 
1984  if (r->getMass() > 0)
1985  {
1986  rnsBodies.push_back(r);
1987  }
1988  }
1989 
1990  RobotNodeSetPtr rnsJ = RobotNodeSet::createRobotNodeSet(br->getRobot(), nameJoints, rnsJoints);
1991  RobotNodeSetPtr rnsB = RobotNodeSet::createRobotNodeSet(br->getRobot(), nameBodies, rnsBodies);
1992 
1993  robotLogger.reset(new BulletRobotLogger(bulletEngine, br, rnsJ, rnsB));
1994  robotLogger->setLogPath(logFile);
1995  robotLogger->startLogging();
1996 }
1997 
1998 
1999 
2000 bool BulletPhysicsWorld::synchronizeSceneObjectPoses(SceneObjectPtr currentObjEngine, std::map<std::string, armarx::PoseBasePtr>& objMap)
2001 {
2002  if (!currentObjEngine)
2003  {
2004  return false;
2005  }
2006 
2007  PosePtr p(new Pose(currentObjEngine->getGlobalPose()));
2008  objMap[currentObjEngine->getName()] = p;
2009  std::vector<SceneObjectPtr> childrenE = currentObjEngine->getChildren();
2010 
2011  for (const auto& i : childrenE)
2012  {
2013  if (!synchronizeSceneObjectPoses(i, objMap))
2014  {
2015  return false;
2016  }
2017  }
2018 
2019  return true;
2020 }
2021 
2022 
2023 bool BulletPhysicsWorld::synchronizeSimulationDataEngine()
2024 {
2025  // lock engine and data
2026  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
2027 
2028  // CONTACTS
2029  if (collectContacts)
2030  {
2031  contacts = dynamicsWorld->getEngine()->getContacts();
2032  std::stringstream ss;
2033  for (auto c : contacts)
2034  {
2035  ss << c.objectAName << " < - > " << c.objectBName;
2036  }
2037  ARMARX_DEBUG << "Contacts : " << endl << ss.str();
2038  }
2039  return true;
2040 }
2041 
2042 btScalar BulletPhysicsWorld::getDeltaTimeMicroseconds()
2043 {
2044  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
2045  btScalar dt = static_cast<btScalar>(m_clock.getTimeMicroseconds());
2046  m_clock.reset();
2047  return dt;
2048 }
2049 
2050 
2051 int BulletPhysicsWorld::getContactCount()
2052 {
2053  ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
2054  return static_cast<int>(contacts.size());
2055 }
2056 
2057 DistanceInfo BulletPhysicsWorld::getRobotNodeDistance(const std::string& robotName, const std::string& robotNodeName1, const std::string& robotNodeName2)
2058 {
2059  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
2060 
2061  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
2062  if (!dynamicsRobot)
2063  {
2064  throw LocalException("No robot");
2065  }
2066  if (dynamicsRobot->getName() != robotName)
2067  {
2068  throw LocalException("Wrong robot name. '") << robotName << "' != '" << dynamicsRobot->getName() << "'";
2069  }
2070  VirtualRobot::RobotPtr robot = dynamicsRobot->getRobot();
2071  if (!robot->hasRobotNode(robotNodeName1))
2072  {
2073  throw LocalException("Wrong robot node name. '") << robotNodeName1 << "'";
2074  }
2075  if (!robot->hasRobotNode(robotNodeName2))
2076  {
2077  throw LocalException("Wrong robot node name. '") << robotNodeName2 << "'";
2078  }
2079 
2080  // Compute distance from node to the rest of the robot (does that make sense?!)
2081  Eigen::Vector3f p1, p2;
2082  auto model1 = robot->getRobotNode(robotNodeName1)->getCollisionModel();
2083  auto model2 = robot->getRobotNode(robotNodeName2)->getCollisionModel();
2084  float d = robot->getCollisionChecker()->calculateDistance(model1, model2, p1, p2);
2085  DistanceInfo di;
2086  di.distance = d;
2087  di.p1 = new Vector3(p1);
2088  di.p2 = new Vector3(p2);
2089 
2090  return di;
2091 }
2092 
2093 DistanceInfo BulletPhysicsWorld::getDistance(const std::string& robotName, const std::string& robotNodeName, const std::string& worldObjectName)
2094 {
2095  ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
2096 
2097  SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
2098  if (!dynamicsRobot)
2099  {
2100  throw LocalException("No robot");
2101  }
2102  if (dynamicsRobot->getName() != robotName)
2103  {
2104  throw LocalException("Wrong robot name. '") << robotName << "' != '" << dynamicsRobot->getName() << "'";
2105  }
2106  VirtualRobot::RobotPtr robot = dynamicsRobot->getRobot();
2107  if (!robot->hasRobotNode(robotNodeName))
2108  {
2109  throw LocalException("Wrong robot node name. '") << robotNodeName << "'";
2110  }
2111  SimDynamics::DynamicsObjectPtr object = getObject(worldObjectName);
2112  if (!object)
2113  {
2114  throw LocalException("No object with name '") << worldObjectName << "'";
2115  }
2116  VirtualRobot::SceneObjectPtr so = object->getSceneObject();
2117  Eigen::Vector3f p1, p2;
2118 
2119  float d = robot->getCollisionChecker()->calculateDistance(robot->getRobotNode(robotNodeName)->getCollisionModel(), so->getCollisionModel(), p1, p2);
2120  DistanceInfo di;
2121  di.distance = d;
2122  di.p1 = new Vector3(p1);
2123  di.p2 = new Vector3(p2);
2124 
2125  return di;
2126 }
2127 
2128 RobotPtr BulletPhysicsWorld::adaptRobotToWorld(VirtualRobot::RobotPtr robot)
2129 {
2130  return RobotFactory::createFlattenedModel(*robot);
2131 }
armarx::BulletPhysicsWorld::applyTorqueObject
void applyTorqueObject(const std::string &objectName, const Eigen::Vector3f &torque) override
Definition: BulletPhysicsWorld.cpp:432
armarx::BulletPhysicsWorld::setObjectPose
void setObjectPose(const std::string &objectName, const Eigen::Matrix4f &globalPose) override
Definition: BulletPhysicsWorld.cpp:470
armarx::SimulatedWorld::simStepExecutionDurationMS
float simStepExecutionDurationMS
Definition: SimulatedWorld.h:457
armarx::SimulatedWorldData::robots
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::vector< RobotInfo > robots
Definition: SimulatedWorld.h:110
armarx::BulletPhysicsWorld::stepSizeMs
int stepSizeMs
Definition: BulletPhysicsWorld.h:201
armarx::SimulatedWorld::simVisuData
SceneVisuData simVisuData
Definition: SimulatedWorld.h:451
armarx::BulletPhysicsWorld::synchronizeSceneObjectPoses
bool synchronizeSceneObjectPoses(VirtualRobot::SceneObjectPtr currentObjEngine, std::map< std::string, armarx::PoseBasePtr > &objMap) override
Definition: BulletPhysicsWorld.cpp:2000
armarx::BulletPhysicsWorld::~BulletPhysicsWorld
~BulletPhysicsWorld() override
Definition: BulletPhysicsWorld.cpp:52
armarx::BulletPhysicsWorld::actuateRobotJoints
void actuateRobotJoints(const std::string &robotName, const std::map< std::string, float > &angles, const std::map< std::string, float > &velocities) override
Definition: BulletPhysicsWorld.cpp:93
armarx::SimulatedWorld::getScopedSyncLock
virtual ScopedRecursiveLockPtr getScopedSyncLock(const std::string &callStr)
Definition: SimulatedWorld.cpp:747
armarx::SimulatedWorld::collectContacts
bool collectContacts
Definition: SimulatedWorld.h:439
armarx::ForceTorqueInfo::robotName
std::string robotName
Definition: SimulatedWorld.h:57
armarx::BulletPhysicsWorld::addRobotEngine
bool addRobotEngine(VirtualRobot::RobotPtr robot, double pid_p, double pid_i, double pid_d, const std::string &filename, bool staticRobot, bool selfCollisions) override
Definition: BulletPhysicsWorld.cpp:1381
LocalException.h
armarx::BulletPhysicsWorld::getDistance
armarx::DistanceInfo getDistance(const std::string &robotName, const std::string &robotNodeName, const std::string &worldObjectName) override
Definition: BulletPhysicsWorld.cpp:2093
armarx::BulletPhysicsWorld::getEngine
SimDynamics::BulletEnginePtr getEngine()
Definition: BulletPhysicsWorld.cpp:1918
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::BulletPhysicsWorld::getRobotLinearVelocity
Eigen::Vector3f getRobotLinearVelocity(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:926
armarx::BulletPhysicsWorld::getRobotJointLimitLo
float getRobotJointLimitLo(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:741
armarx::BulletPhysicsWorld::objectGraspedEngine
bool objectGraspedEngine(const std::string &robotName, const std::string &robotNodeName, const std::string &objectName, Eigen::Matrix4f &storeLocalTransform) override
create a joint
Definition: BulletPhysicsWorld.cpp:1295
robots
std::vector< VirtualRobot::RobotPtr > robots
Definition: VisualizationRobot.cpp:85
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
VirtualRobot
Definition: FramedPose.h:43
armarx::BulletPhysicsWorld::synchronizeSimulationDataEngine
bool synchronizeSimulationDataEngine() override
Definition: BulletPhysicsWorld.cpp:2023
armarx::BulletPhysicsWorld::DynamicsRobotInfo::robot
VirtualRobot::RobotPtr robot
Definition: BulletPhysicsWorld.h:209
armarx::BulletPhysicsWorld::setRobotAngularVelocity
void setRobotAngularVelocity(const std::string &robotName, const std::string &robotNodeName, const Eigen::Vector3f &vel) override
Definition: BulletPhysicsWorld.cpp:1038
armarx::BulletPhysicsWorld::getFixedTimeStepMS
int getFixedTimeStepMS() override
Definition: BulletPhysicsWorld.cpp:1912
armarx::BulletPhysicsWorld::dynamicsWorld
SimDynamics::DynamicsWorldPtr dynamicsWorld
Definition: BulletPhysicsWorld.h:215
armarx::BulletPhysicsWorld::getRobotForceTorqueSensors
ForceTorqueDataSeq getRobotForceTorqueSensors(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:696
armarx::BulletPhysicsWorld::removeObstacleEngine
bool removeObstacleEngine(const std::string &name) override
Definition: BulletPhysicsWorld.cpp:1775
armarx::BulletPhysicsWorld::getObjects
std::vector< VirtualRobot::SceneObjectPtr > getObjects() override
Definition: BulletPhysicsWorld.cpp:1924
armarx::BulletPhysicsWorld::stepPhysicsRealTime
void stepPhysicsRealTime() override
Perform one simulation step.
Definition: BulletPhysicsWorld.cpp:1806
armarx::BulletPhysicsWorld::getRobotStatus
bool getRobotStatus(const std::string &robotName, NameValueMap &jointAngles, NameValueMap &jointVelocities, NameValueMap &jointTorques, Eigen::Vector3f &linearVelocity, Eigen::Vector3f &angularVelocity) override
Definition: BulletPhysicsWorld.cpp:1526
armarx::SimulatedWorld::maxRealTimeSimSpeed
float maxRealTimeSimSpeed
Definition: SimulatedWorld.h:453
armarx::FramedPosePtr
IceInternal::Handle< FramedPose > FramedPosePtr
Definition: FramedPose.h:250
armarx::ForceTorqueInfo
Definition: SimulatedWorld.h:48
armarx::BulletPhysicsWorld::getRobotAngularVelocity
Eigen::Vector3f getRobotAngularVelocity(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:993
armarx::SimulatedWorld
The SimulatedWorld class encapsulates the whole physics simulation and the corresponding data.
Definition: SimulatedWorld.h:128
armarx::BulletPhysicsWorld::setRobotMaxTorque
void setRobotMaxTorque(const std::string &robotName, const std::string &nodeName, float maxTorque) override
Definition: BulletPhysicsWorld.cpp:861
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::SimulatedWorld::simTimeStepMS
float simTimeStepMS
Definition: SimulatedWorld.h:454
armarx::BulletPhysicsWorld::getRobotJointLimitHi
float getRobotJointLimitHi(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:769
armarx::SimulatedWorld::attachedObjects
std::vector< GraspingInfo > attachedObjects
Definition: SimulatedWorld.h:449
so
linux gnu libIceDB so
Definition: CMakeLists.txt:7
armarx::BulletPhysicsWorld::stepStaticRobots
void stepStaticRobots(double deltaInSeconds)
Definition: BulletPhysicsWorld.cpp:1877
armarx::BulletPhysicsWorld::applyTorqueRobotNode
void applyTorqueRobotNode(const std::string &robotName, const std::string &robotNodeName, const Eigen::Vector3f &torque) override
Definition: BulletPhysicsWorld.cpp:386
armarx::SimulatedWorld::synchronizeDurationMS
float synchronizeDurationMS
Definition: SimulatedWorld.h:459
armarx::BulletPhysicsWorld::stepPhysicsFixedTimeStep
void stepPhysicsFixedTimeStep() override
Perform one simulation step.
Definition: BulletPhysicsWorld.cpp:1834
armarx::BulletPhysicsWorld::getRobotNames
std::vector< std::string > getRobotNames() override
Definition: BulletPhysicsWorld.cpp:1715
armarx::BulletPhysicsWorld::getDynamicObjects
std::vector< SimDynamics::DynamicsObjectPtr > getDynamicObjects()
Definition: BulletPhysicsWorld.cpp:1934
armarx::SimulatedWorld::simReportData
SimulatedWorldData simReportData
Definition: SimulatedWorld.h:452
armarx::BulletPhysicsWorld::getFirstDynamicsObject
SimDynamics::DynamicsObjectPtr getFirstDynamicsObject(const std::string &robotName, const std::string &nodeName)
Definition: BulletPhysicsWorld.cpp:941
armarx::SimulatedWorld::getScopedEngineLock
virtual ScopedRecursiveLockPtr getScopedEngineLock(const std::string &callStr)
Definition: SimulatedWorld.cpp:714
armarx::BulletPhysicsWorld::setupFloorEngine
void setupFloorEngine(bool enable, const std::string &floorTexture) override
Definition: BulletPhysicsWorld.cpp:1939
armarx::ForceTorqueInfo::robotNodeName
std::string robotNodeName
Definition: SimulatedWorld.h:58
IceInternal::Handle< FramedPose >
armarx::BulletPhysicsWorld::getRobotMaxTorque
float getRobotMaxTorque(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:826
armarx::BulletPhysicsWorld::setRobotLinearVelocityRobotRootFrame
void setRobotLinearVelocityRobotRootFrame(const std::string &robotName, const std::string &robotNodeName, const Eigen::Vector3f &vel) override
Definition: BulletPhysicsWorld.cpp:1068
armarx::BulletPhysicsWorld::DynamicsRobotInfo
Definition: BulletPhysicsWorld.h:207
armarx::BulletPhysicsWorld::updateForceTorqueSensor
bool updateForceTorqueSensor(ForceTorqueInfo &ftInfo) override
Definition: BulletPhysicsWorld.cpp:1588
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::BulletPhysicsWorld::activateObject
void activateObject(const std::string &objectName) override
Definition: BulletPhysicsWorld.cpp:484
armarx::BulletPhysicsWorld::synchronizeObjects
bool synchronizeObjects() override
Definition: BulletPhysicsWorld.cpp:1640
armarx::BulletPhysicsWorld::synchronizeRobotNodePoses
bool synchronizeRobotNodePoses(const std::string &robotName, std::map< std::string, armarx::PoseBasePtr > &objMap) override
Definition: BulletPhysicsWorld.cpp:1501
armarx::ForceTorqueInfo::currentForce
Eigen::Vector3f currentForce
Definition: SimulatedWorld.h:62
armarx::ForceTorqueInfo::currentTorque
Eigen::Vector3f currentTorque
Definition: SimulatedWorld.h:63
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:198
armarx::BulletPhysicsWorld::setRobotAngularVelocityRobotRootFrame
void setRobotAngularVelocityRobotRootFrame(const std::string &robotName, const std::string &robotNodeName, const Eigen::Vector3f &vel) override
Definition: BulletPhysicsWorld.cpp:1112
armarx::BulletPhysicsWorld::actuateRobotJointsVel
void actuateRobotJointsVel(const std::string &robotName, const std::map< std::string, float > &velocities) override
Definition: BulletPhysicsWorld.cpp:240
armarx::SimulatedWorld::engineMutex
MutexPtrType engineMutex
Definition: SimulatedWorld.h:436
armarx::SimulatedWorld::ScopedRecursiveLockPtr
std::shared_ptr< ScopedRecursiveLock > ScopedRecursiveLockPtr
Definition: SimulatedWorld.h:319
armarx::BulletPhysicsWorld::getObjectPose
Eigen::Matrix4f getObjectPose(const std::string &objectName) override
Definition: BulletPhysicsWorld.cpp:1243
ARMARX_ERROR_S
#define ARMARX_ERROR_S
Definition: Logging.h:209
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::BulletPhysicsWorld::getDeltaTimeMicroseconds
btScalar getDeltaTimeMicroseconds()
Definition: BulletPhysicsWorld.cpp:2042
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::BulletPhysicsWorld::getObject
SimDynamics::DynamicsObjectPtr getObject(const std::string &objectName)
Definition: BulletPhysicsWorld.cpp:447
armarx::BulletPhysicsWorld::hasRobot
bool hasRobot(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:1158
armarx::BulletPhysicsWorld::removeRobotEngine
bool removeRobotEngine(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:1352
armarx::BulletPhysicsWorld::getRobot
VirtualRobot::RobotPtr getRobot(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:1190
armarx::ForceTorqueInfo::ftSensor
VirtualRobot::ForceTorqueSensorPtr ftSensor
Definition: SimulatedWorld.h:60
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
armarx::BulletPhysicsWorld::enableLogging
void enableLogging(const std::string &robotName, const std::string &logFile) override
Definition: BulletPhysicsWorld.cpp:1958
armarx::BulletPhysicsWorld::getRobotJointVelocities
std::map< std::string, float > getRobotJointVelocities(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:590
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::BulletPhysicsWorld::DynamicsRobotInfo::targetVelocities
std::map< std::string, float > targetVelocities
Definition: BulletPhysicsWorld.h:212
armarx::BulletPhysicsWorld::adaptRobotToWorld
VirtualRobot::RobotPtr adaptRobotToWorld(VirtualRobot::RobotPtr robot) override
Definition: BulletPhysicsWorld.cpp:2128
armarx::channels::KinematicUnitObserver::jointVelocities
const KinematicUnitDatafieldCreator jointVelocities("jointVelocities")
armarx::BulletPhysicsWorld::getContactCount
int getContactCount() override
Definition: BulletPhysicsWorld.cpp:2051
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::BulletPhysicsWorld::getRobotJointVelocity
float getRobotJointVelocity(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:625
armarx::BulletPhysicsWorld::hasObject
bool hasObject(const std::string &instanceName) override
Definition: BulletPhysicsWorld.cpp:465
armarx::BulletPhysicsWorld::bulletFixedTimeStepMS
int bulletFixedTimeStepMS
Definition: BulletPhysicsWorld.h:200
armarx::BulletPhysicsWorld::dynamicsObjects
std::vector< SimDynamics::DynamicsObjectPtr > dynamicsObjects
Definition: BulletPhysicsWorld.h:217
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::BulletPhysicsWorld::robotLogger
SimDynamics::BulletRobotLoggerPtr robotLogger
Definition: BulletPhysicsWorld.h:219
armarx::BulletPhysicsWorld::getRobotPose
Eigen::Matrix4f getRobotPose(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:797
armarx::BulletPhysicsWorld::m_clock
btClock m_clock
Definition: BulletPhysicsWorld.h:205
armarx::BulletPhysicsWorld::getRobotJointAngle
float getRobotJointAngle(const std::string &robotName, const std::string &nodeName) override
Definition: BulletPhysicsWorld.cpp:560
armarx::BulletPhysicsWorld::setRobotPose
void setRobotPose(const std::string &robotName, const Eigen::Matrix4f &globalPose) override
Definition: BulletPhysicsWorld.cpp:331
armarx::BulletPhysicsWorld::actuateRobotJointsPos
void actuateRobotJointsPos(const std::string &robotName, const std::map< std::string, float > &angles) override
Definition: BulletPhysicsWorld.cpp:168
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:91
armarx::channels::KinematicUnitObserver::jointAngles
const KinematicUnitDatafieldCreator jointAngles("jointAngles")
armarx::BulletPhysicsWorld::toFramedPose
FramedPosePtr toFramedPose(const Eigen::Matrix4f &globalPose, const std::string &robotName, const std::string &frameName) override
toFramedPose Constructs a framed pose
Definition: BulletPhysicsWorld.cpp:1475
armarx::BulletPhysicsWorld::getRobotJointTorques
std::map< std::string, float > getRobotJointTorques(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:655
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::BulletPhysicsWorld::copyContacts
std::vector< SimDynamics::DynamicsEngine::DynamicsContactInfo > copyContacts() override
Definition: BulletPhysicsWorld.cpp:1672
armarx::BulletPhysicsWorld::bulletEngine
SimDynamics::BulletEnginePtr bulletEngine
Definition: BulletPhysicsWorld.h:198
armarx::BulletPhysicsWorld::actuateRobotJointsTorque
void actuateRobotJointsTorque(const std::string &robotName, const std::map< std::string, float > &torques) override
Definition: BulletPhysicsWorld.cpp:292
armarx::viz::data::ElementFlags::names
const simox::meta::IntEnumNames names
Definition: json_elements.cpp:14
armarx::BulletPhysicsWorld::applyForceRobotNode
void applyForceRobotNode(const std::string &robotName, const std::string &robotNodeName, const Eigen::Vector3f &force) override
Definition: BulletPhysicsWorld.cpp:355
armarx::BulletPhysicsWorld::setObjectSimType
void setObjectSimType(const std::string &objectName, VirtualRobot::SceneObject::Physics::SimulationType simType) override
Definition: BulletPhysicsWorld.cpp:1730
armarx::BulletPhysicsWorld::DynamicsRobotInfo::isStatic
bool isStatic
Definition: BulletPhysicsWorld.h:211
armarx::BulletPhysicsWorld::setRobotNodeSimType
void setRobotNodeSimType(const std::string &robotName, const std::string &robotNodeName, VirtualRobot::SceneObject::Physics::SimulationType simType) override
Definition: BulletPhysicsWorld.cpp:1742
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::BulletPhysicsWorld::getObstacleNames
std::vector< std::string > getObstacleNames() override
Definition: BulletPhysicsWorld.cpp:1760
armarx::SimulatedWorld::currentSimTimeSec
double currentSimTimeSec
Definition: SimulatedWorld.h:433
armarx::BulletPhysicsWorld::addObstacleEngine
bool addObstacleEngine(VirtualRobot::SceneObjectPtr o, VirtualRobot::SceneObject::Physics::SimulationType simType) override
Definition: BulletPhysicsWorld.cpp:1678
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
armarx::BulletPhysicsWorld::initialize
virtual void initialize(int stepSizeMS, int bulletFixedTimeStepMS, int bulletFixedTimeStepMaxNrLoops, float maxRealTimeSimSpeed=1, bool floorPlane=false, std::string floorTexture=std::string())
Definition: BulletPhysicsWorld.cpp:58
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:200
armarx::BulletPhysicsWorld::getRobots
std::map< std::string, VirtualRobot::RobotPtr > getRobots() override
Definition: BulletPhysicsWorld.cpp:1202
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
armarx::BulletPhysicsWorld::getRobotNodePose
Eigen::Matrix4f getRobotNodePose(const std::string &robotName, const std::string &robotNodeName) override
Definition: BulletPhysicsWorld.cpp:896
armarx::SimulatedWorld::setupFloor
virtual void setupFloor(bool enable, const std::string &floorTexture)
Definition: SimulatedWorld.cpp:780
armarx::BulletPhysicsWorld::getFloor
VirtualRobot::SceneObjectPtr getFloor() override
Definition: BulletPhysicsWorld.cpp:1341
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::BulletPhysicsWorld::objectReleasedEngine
bool objectReleasedEngine(const std::string &robotName, const std::string &robotNodeName, const std::string &objectName) override
remove a joint
Definition: BulletPhysicsWorld.cpp:1258
Logging.h
armarx::channels::KinematicUnitObserver::jointTorques
const KinematicUnitDatafieldCreator jointTorques("jointTorques")
armarx::BulletPhysicsWorld::DynamicsRobotInfo::dynamicsRobot
SimDynamics::DynamicsRobotPtr dynamicsRobot
Definition: BulletPhysicsWorld.h:210
armarx::BulletPhysicsWorld::getRobotNodeDistance
armarx::DistanceInfo getRobotNodeDistance(const std::string &robotName, const std::string &robotNodeName1, const std::string &robotNodeName2) override
Definition: BulletPhysicsWorld.cpp:2057
armarx::BulletPhysicsWorld::setRobotLinearVelocity
void setRobotLinearVelocity(const std::string &robotName, const std::string &robotNodeName, const Eigen::Vector3f &vel) override
Definition: BulletPhysicsWorld.cpp:1008
armarx::BulletPhysicsWorld::bulletFixedTimeStepMaxNrLoops
int bulletFixedTimeStepMaxNrLoops
Definition: BulletPhysicsWorld.h:199
ArmarXDataPath.h
armarx::BulletPhysicsWorld::getDynamicsObject
SimDynamics::DynamicsObjectPtr getDynamicsObject(const std::string &objectName)
Definition: BulletPhysicsWorld.cpp:1226
armarx::BulletPhysicsWorld::hasRobotNode
bool hasRobotNode(const std::string &robotName, const std::string &robotNodeName) override
Definition: BulletPhysicsWorld.cpp:1176
armarx::ForceTorqueInfo::enable
bool enable
Definition: SimulatedWorld.h:61
armarx::BulletPhysicsWorld::getRobotJointAngles
std::map< std::string, float > getRobotJointAngles(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:525
armarx::BulletPhysicsWorld::contacts
std::vector< SimDynamics::DynamicsEngine::DynamicsContactInfo > contacts
Definition: BulletPhysicsWorld.h:203
armarx::BulletPhysicsWorld::getDynamicRobot
virtual SimDynamics::DynamicsRobotPtr getDynamicRobot(const std::string &robotName)
Definition: BulletPhysicsWorld.cpp:1214
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::BulletPhysicsWorld::getRobotMass
float getRobotMass(const std::string &robotName) override
Definition: BulletPhysicsWorld.cpp:503
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
BulletPhysicsWorld.h
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:42
armarx::BulletPhysicsWorld::dynamicRobots
std::map< std::string, DynamicsRobotInfo > dynamicRobots
Definition: BulletPhysicsWorld.h:216
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::BulletPhysicsWorld::applyForceObject
void applyForceObject(const std::string &objectName, const Eigen::Vector3f &force) override
Definition: BulletPhysicsWorld.cpp:418