KinematicsWorld.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       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
// just needed for SceneData
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
// #include <ArmarXCore/core/system/Synchronization.h>
#include <mutex>
 
#include <ArmarXCore/observers/filters/DerivationFilter.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include "SimulatedWorld.h"
 
namespace armarx
{
    /*!
     * \brief The KinematicsWorld class encapsulates the kinemtics simulation and the corresponding data.
     * All real physics simulation is either approximated through simple kinemtics or disabled.
     *
     */
    class KinematicsWorld : public SimulatedWorld
    {
    public:
        KinematicsWorld();
        ~KinematicsWorld() override;
 
        virtual void initialize(int stepSizeMS,
                                float maxRealTimeSimSpeed = 1,
                                bool floorPlane = false,
                                std::string floorTexture = std::string());
 
        //! ignoring velocity target
        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;
 
        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&) override;
        ForceTorqueDataSeq getRobotForceTorqueSensors(const std::string&) 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;
 
        // the robotNodeName is ignored, the whole robot is moved...
        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;
 
        VirtualRobot::RobotPtr getRobot(const std::string& robotName) override;
        std::map<std::string, VirtualRobot::RobotPtr> getRobots() override;
        virtual VirtualRobot::SceneObjectPtr getObject(const std::string& objectName);
        std::vector<VirtualRobot::SceneObjectPtr> getObjects() override;
 
        std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> copyContacts() 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;
 
 
        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<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);
        void stepStaticObjects(double deltaInSeconds);
 
        int stepSizeMs;
 
        struct KinematicRobotInfo
        {
            KinematicRobotInfo()
            {
                velTransTarget.setZero();
                velRotTarget.setZero();
                velTransActual.setZero();
                velRotActual.setZero();
            }
 
            VirtualRobot::RobotPtr robot;
            std::map<std::string, float> targetVelocities;
            std::map<std::string, float> actualVelocities;
            Eigen::Vector3f velTransTarget, velTransActual;
            Eigen::Vector3f velRotTarget, velRotActual;
        };
 
        std::map<std::string, KinematicRobotInfo> kinematicRobots;
        std::vector<VirtualRobot::SceneObjectPtr> kinematicObjects;
        std::map<VirtualRobot::RobotNodePtr, std::pair<IceUtil::Time, float>>
            jointAngleDerivationFilters;
        std::map<VirtualRobot::RobotPtr, std::pair<IceUtil::Time, Eigen::Vector3f>>
            linearVelocityFilters;
        std::map<VirtualRobot::RobotPtr, std::pair<IceUtil::Time, Eigen::Matrix4f>>
            angularVelocityFilters;
        VirtualRobot::SceneObjectPtr floor;
        Eigen::Vector3f floorPos;
        Eigen::Vector3f floorUp;
        double floorExtendMM;
        double floorDepthMM;
        VirtualRobot::ObstaclePtr groundObject;
        virtual void createFloorPlane(const Eigen::Vector3f& pos, const Eigen::Vector3f& up);
 
        IceUtil::Time lastTime;
 
        float getDeltaTimeMilli();
 
        bool synchronizeRobotNodePoses(const std::string& robotName,
                                       std::map<std::string, armarx::PoseBasePtr>& objMap) override;
        void calculateActualVelocities();
    };
 
    using KinematicsWorldPtr = std::shared_ptr<KinematicsWorld>;
} // namespace armarx