BulletPhysicsWorld.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXSimulation::Simulator
 * @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <LinearMath/btQuickprof.h>
#include <SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h>
#include <SimDynamics/DynamicsEngine/BulletEngine/BulletRobotLogger.h>
#include <SimDynamics/DynamicsEngine/DynamicsEngine.h>
#include <SimDynamics/DynamicsEngine/DynamicsObject.h>
#include <SimDynamics/DynamicsEngine/DynamicsRobot.h>
#include <SimDynamics/DynamicsWorld.h>
#include <btBulletDynamicsCommon.h>
 
// just needed for SceneData
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
// #include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include "SimulatedWorld.h"
 
namespace armarx
{
    /*!
     * \brief The BulletPhysicsWorld class encapsulates the whole physics simulation and the corresponding data
     *
     */
    class BulletPhysicsWorld : public SimulatedWorld
    {
    public:
        BulletPhysicsWorld();
        ~BulletPhysicsWorld() override;
 
 
        virtual void initialize(int stepSizeMS,
                                int bulletFixedTimeStepMS,
                                int bulletFixedTimeStepMaxNrLoops,
                                float maxRealTimeSimSpeed = 1,
                                bool floorPlane = false,
                                std::string floorTexture = std::string());
 
        VirtualRobot::RobotPtr adaptRobotToWorld(VirtualRobot::RobotPtr robot) override;
 
        void actuateRobotJoints(const std::string& robotName,
                                const std::map<std::string, float>& angles,
                                const std::map<std::string, float>& velocities) override;
        void actuateRobotJointsPos(const std::string& robotName,
                                   const std::map<std::string, float>& angles) override;
        void actuateRobotJointsVel(const std::string& robotName,
                                   const std::map<std::string, float>& velocities) override;
        void actuateRobotJointsTorque(const std::string& robotName,
                                      const std::map<std::string, float>& torques) override;
        void setRobotPose(const std::string& robotName, const Eigen::Matrix4f& globalPose) override;
 
        void applyForceRobotNode(const std::string& robotName,
                                 const std::string& robotNodeName,
                                 const Eigen::Vector3f& force) override;
        void applyTorqueRobotNode(const std::string& robotName,
                                  const std::string& robotNodeName,
                                  const Eigen::Vector3f& torque) override;
 
        void applyForceObject(const std::string& objectName, const Eigen::Vector3f& force) override;
        void applyTorqueObject(const std::string& objectName,
                               const Eigen::Vector3f& torque) override;
 
        bool hasObject(const std::string& instanceName) override;
        void setObjectPose(const std::string& objectName,
                           const Eigen::Matrix4f& globalPose) override;
        void activateObject(const std::string& objectName) override;
 
        bool hasRobot(const std::string& robotName) override;
        bool hasRobotNode(const std::string& robotName, const std::string& robotNodeName) override;
        float getRobotMass(const std::string& robotName) override;
 
        std::map<std::string, float> getRobotJointAngles(const std::string& robotName) override;
        float getRobotJointAngle(const std::string& robotName,
                                 const std::string& nodeName) override;
        std::map<std::string, float> getRobotJointVelocities(const std::string& robotName) override;
        float getRobotJointVelocity(const std::string& robotName,
                                    const std::string& nodeName) override;
        std::map<std::string, float> getRobotJointTorques(const std::string& robotName) override;
        ForceTorqueDataSeq getRobotForceTorqueSensors(const std::string& robotName) override;
 
        float getRobotJointLimitLo(const std::string& robotName,
                                   const std::string& nodeName) override;
        float getRobotJointLimitHi(const std::string& robotName,
                                   const std::string& nodeName) override;
        Eigen::Matrix4f getRobotPose(const std::string& robotName) override;
 
        float getRobotMaxTorque(const std::string& robotName, const std::string& nodeName) override;
        void setRobotMaxTorque(const std::string& robotName,
                               const std::string& nodeName,
                               float maxTorque) override;
 
        Eigen::Matrix4f getRobotNodePose(const std::string& robotName,
                                         const std::string& robotNodeName) override;
 
        Eigen::Vector3f getRobotLinearVelocity(const std::string& robotName,
                                               const std::string& nodeName) override;
        Eigen::Vector3f getRobotAngularVelocity(const std::string& robotName,
                                                const std::string& nodeName) override;
 
        void setRobotLinearVelocity(const std::string& robotName,
                                    const std::string& robotNodeName,
                                    const Eigen::Vector3f& vel) override;
        void setRobotAngularVelocity(const std::string& robotName,
                                     const std::string& robotNodeName,
                                     const Eigen::Vector3f& vel) override;
        void setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
                                                  const std::string& robotNodeName,
                                                  const Eigen::Vector3f& vel) override;
        void setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
                                                   const std::string& robotNodeName,
                                                   const Eigen::Vector3f& vel) override;
 
        Eigen::Matrix4f getObjectPose(const std::string& objectName) override;
 
 
        FramedPosePtr toFramedPose(const Eigen::Matrix4f& globalPose,
                                   const std::string& robotName,
                                   const std::string& frameName) override;
 
        /**
        * Perform one simulation step. Calculates the delta update time from the time that has passed since the last call.
        * This updates all models.
        */
        void stepPhysicsRealTime() override;
 
        /**
        * Perform one simulation step.
        * This updates all models.
        */
        void stepPhysicsFixedTimeStep() override;
 
        /*!
         * \brief
         * \return The number of steps * the timestep in MS
         */
        int getFixedTimeStepMS() override;
 
        SimDynamics::BulletEnginePtr getEngine();
 
        std::vector<VirtualRobot::SceneObjectPtr> getObjects() override;
 
        std::vector<SimDynamics::DynamicsObjectPtr> getDynamicObjects();
        SimDynamics::DynamicsObjectPtr getObject(const std::string& objectName);
 
 
        SimDynamics::DynamicsObjectPtr getDynamicsObject(const std::string& objectName);
 
        void enableLogging(const std::string& robotName, const std::string& logFile) override;
 
        armarx::DistanceInfo getRobotNodeDistance(const std::string& robotName,
                                                  const std::string& robotNodeName1,
                                                  const std::string& robotNodeName2) override;
        armarx::DistanceInfo getDistance(const std::string& robotName,
                                         const std::string& robotNodeName,
                                         const std::string& worldObjectName) override;
 
        std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> copyContacts() override;
 
 
        int getContactCount() override;
 
        VirtualRobot::RobotPtr getRobot(const std::string& robotName) override;
        std::map<std::string, VirtualRobot::RobotPtr> getRobots() override;
        virtual SimDynamics::DynamicsRobotPtr getDynamicRobot(const std::string& robotName);
 
        std::vector<std::string> getRobotNames() override;
        std::vector<std::string> getObstacleNames() override;
 
        void
        setRobotNodeSimType(const std::string& robotName,
                            const std::string& robotNodeName,
                            VirtualRobot::SceneObject::Physics::SimulationType simType) override;
        void setObjectSimType(const std::string& objectName,
                              VirtualRobot::SceneObject::Physics::SimulationType simType) override;
 
    protected:
        bool addObstacleEngine(VirtualRobot::SceneObjectPtr o,
                               VirtualRobot::SceneObject::Physics::SimulationType simType) override;
        bool removeObstacleEngine(const std::string& name) override;
        bool addRobotEngine(VirtualRobot::RobotPtr robot,
                            double pid_p,
                            double pid_i,
                            double pid_d,
                            const std::string& filename,
                            bool staticRobot,
                            bool selfCollisions) override;
 
        //! create a joint
        bool objectGraspedEngine(const std::string& robotName,
                                 const std::string& robotNodeName,
                                 const std::string& objectName,
                                 Eigen::Matrix4f& storeLocalTransform) override;
 
        //! remove a joint
        bool objectReleasedEngine(const std::string& robotName,
                                  const std::string& robotNodeName,
                                  const std::string& objectName) override;
 
        bool removeRobotEngine(const std::string& robotName) override;
        bool synchronizeSimulationDataEngine() override;
 
        void setupFloorEngine(bool enable, const std::string& floorTexture) override;
 
        bool synchronizeObjects() override;
 
        bool getRobotStatus(const std::string& robotName,
                            NameValueMap& jointAngles,
                            NameValueMap& jointVelocities,
                            NameValueMap& jointTorques,
                            Eigen::Vector3f& linearVelocity,
                            Eigen::Vector3f& angularVelocity) override;
 
        bool updateForceTorqueSensor(ForceTorqueInfo& ftInfo) override;
 
        bool
        synchronizeSceneObjectPoses(VirtualRobot::SceneObjectPtr currentObjEngine,
                                    std::map<std::string, armarx::PoseBasePtr>& objMap) override;
 
        VirtualRobot::SceneObjectPtr getFloor() override;
 
        // manually apply velocities to static robots
        void stepStaticRobots(double deltaInSeconds);
 
        // we assume to have a bullet engine
        SimDynamics::BulletEnginePtr bulletEngine;
        int bulletFixedTimeStepMaxNrLoops;
        int bulletFixedTimeStepMS;
        int stepSizeMs;
 
        std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts;
 
        btClock m_clock;
 
        struct DynamicsRobotInfo
        {
            VirtualRobot::RobotPtr robot;
            SimDynamics::DynamicsRobotPtr dynamicsRobot;
            bool isStatic;
            std::map<std::string, float> targetVelocities; // only needed for static roobts
        };
 
        SimDynamics::DynamicsWorldPtr dynamicsWorld;
        std::map<std::string, DynamicsRobotInfo> dynamicRobots;
        std::vector<SimDynamics::DynamicsObjectPtr> dynamicsObjects;
 
        SimDynamics::BulletRobotLoggerPtr robotLogger;
 
        btScalar getDeltaTimeMicroseconds();
 
        SimDynamics::DynamicsObjectPtr getFirstDynamicsObject(const std::string& robotName,
                                                              const std::string& nodeName);
 
        bool synchronizeRobotNodePoses(const std::string& robotName,
                                       std::map<std::string, armarx::PoseBasePtr>& objMap) override;
    };
 
    using BulletPhysicsWorldPtr = std::shared_ptr<BulletPhysicsWorld>;
} // namespace armarx