TrajectoryManager.h

Go to the documentation of this file.

#ifndef TRAJECTORYMANAGER_H
#define TRAJECTORYMANAGER_H

#include "robodyn_controllers/JointTrajectoryManager.h"
#include "robodyn_utilities/GoalStatusSender.h"
#include <sensor_msgs/JointState.h>
#include <boost/ptr_container/ptr_map.hpp>
#include "robodyn_controllers/JointTrajectoryFollower.h"
#include "r2_msgs/PoseTrajectory.h"
#include "r2_msgs/ControllerPoseSettings.h"
#include "r2_msgs/PoseState.h"
#include "r2_msgs/StringArray.h"
#include "robodyn_controllers/MobileTreeIk.h"
#include "robodyn_controllers/TrapezoidalVelocityTrajectoryFactory.h"
#include "robodyn_controllers/RosMsgTrajectoryFactory.h"
#include "robodyn_controllers/RosMsgTreeIkTrajectoryFactory.h"
#include "robodyn_controllers/RosMsgForceTrajectoryFactory.h"
#include "r2_msgs/JointCommand.h"
#include "r2_msgs/JointControlDataArray.h"
#include "r2_msgs/ReplanType.h"
#include "robodyn_controllers/MotionLimiter.h"
#include "r2_msgs/JointCapability.h"
#include "r2_msgs/ForceControlAxis.h"
#include "r2_msgs/WrenchState.h"
#include "r2_msgs/ForceControlAxisArray.h"
#include "r2_msgs/ToolFrame.h"
#include "robodyn_controllers/RosMsgToolFrameManager.h"

#include <iostream>
#include <string>

class TrajectoryManager : public JointTrajectoryManager, public GoalStatusSender
{
public:
    TrajectoryManager(const std::string& name);

    virtual ~TrajectoryManager();

    struct ForceGains
    {
      double filterAlpha;
      double forceGain;
      double torqueGain;
      double forceIntegral;
      double torqueIntegral;
      double forceDamping;
      double torqueDamping;
      double forceWindupLimit;
      double torqueWindupLimit;
      double positionErrorLimit;
      double rotationErrorLimit;
      bool doFeedForward;

      ForceGains()
      {
        filterAlpha = 0.9;
        forceGain = torqueGain = 2.0;
        forceIntegral = torqueIntegral = 0.0;
        forceDamping = torqueDamping = 0.2;
        forceWindupLimit = torqueWindupLimit = 0;
        positionErrorLimit = rotationErrorLimit = 0.02;
        doFeedForward = false;
      }
    };

    void initialize(const std::string& urdfFile, double timeStep_in);
    void initialize_sim(const std::string& urdfDesc, double timeStepIn);
    void updateTree(const r2_msgs::ToolFrame& toolFrameMsg);

    void updateTrajectories(const r2_msgs::JointCommand& defaultCommandMsg, const r2_msgs::JointControlDataArray& defaultControlMsg, const sensor_msgs::JointState& startJointPositions, const sensor_msgs::JointState& startJointVels);
    void updateSensorForces(const r2_msgs::WrenchState& wrenchSensors);
    void updateActualPoseState(const r2_msgs::PoseState& actualPoseState);
    void updateInertia(const sensor_msgs::JointState& inertia_in);

    void setJointSettings(const r2_msgs::ControllerJointSettings& settingsMsg);
    void getJointSettings(r2_msgs::ControllerJointSettings& settingsMsg);
    void setPoseSettings(const r2_msgs::ControllerPoseSettings& settingsMsg);
    void setJointCapabilities(const r2_msgs::JointCapability& capabilitiesMsg);
    void setBases(const r2_msgs::StringArray& basesMsg);
    void setVelocityFactor(double velocityFactor_in) {velocityFactor = velocityFactor_in;}
    void setPriorityTol(std::vector<double> priorityNum, std::vector<double> priorityLinearTol, std::vector<double> priorityAngularTol);
    void setIkParameters(double mBar, double kr, unsigned int maxIts, double maxTwist);
    void setSensorNameMap(std::vector<std::string> sensorKeys, std::vector<std::string> sensorNames);
    void setForceParameters(struct ForceGains new_gains);

    double addForceWaypoints(const r2_msgs::PoseTrajectory& trajectory,
                             const sensor_msgs::JointState& startJointPositions,
                             const sensor_msgs::JointState& startJointVels,
                             const sensor_msgs::JointState& prevJointVels,
                             const r2_msgs::PoseState& startPoseState,
                             const r2_msgs::PoseState& startPoseVels,
                             const r2_msgs::ForceControlAxisArray& forceAxes);
    double addCartesianWaypoints(const r2_msgs::PoseTrajectory& trajectory,
                                 const sensor_msgs::JointState& startJointPositions,
                                 const sensor_msgs::JointState& startJointVels,
                                 const sensor_msgs::JointState& prevJointVels,
                                 const r2_msgs::PoseState& startPoseState,
                                 const r2_msgs::PoseState& startPoseVels);
    void addJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
                           const sensor_msgs::JointState& startJointPositions,
                           const sensor_msgs::JointState& startJointVels,
                           const sensor_msgs::JointState& prevJointVels,
                           const std::vector<r2_msgs::ReplanType>& replanVec = std::vector<r2_msgs::ReplanType>());
    void addJointBreadcrumbs(const trajectory_msgs::JointTrajectory& trajectory);

protected:
    virtual void writeJointState(const sensor_msgs::JointState& jointState_out) = 0;
    virtual void writeJointControl(const r2_msgs::JointControlDataArray& jointControl_out) = 0;

    bool isActive(const actionlib_msgs::GoalID& goalId) const;

    // helper functions
    void abort(const actionlib_msgs::GoalID& goalId, const std::string& msg = std::string()); // abort if active
    void abort(const actionlib_msgs::GoalID& goalId, std::set<std::string>& abortedJoints, const std::string& msg = std::string()); // abort if active and return joint names aborted
    void preempt(const std::vector<std::string>& jointNames, std::set<std::string>& preemptedJoints, const std::string& msg = std::string()); // preempt if joint being used
    void resetAll(const std::string& msg = std::string());
    // kasquish is a smooth stop
    void kasquish(const std::vector<std::string>& jointNames,
                  const sensor_msgs::JointState& startJointPositions,
                  const sensor_msgs::JointState& startJointVels);
    void kasquishAll(const sensor_msgs::JointState& startJointPositions,
                     const sensor_msgs::JointState& startJointVels, const std::string& msg = std::string());

    std::string sensor;

    double velocityFactor;
    double timeStep;
    double stopVelocityLimit;
    double stopAccelerationLimit;
    double zeroLimit;
    ros::Duration timestep_in;

    bool jointSettingsValid, poseSettingsValid, basesValid;


private:
    struct GoalIdCompare : public std::binary_function<actionlib_msgs::GoalID, actionlib_msgs::GoalID, bool>
    {
        bool operator()(const actionlib_msgs::GoalID& left, const actionlib_msgs::GoalID& right) const
        {
            return (left.stamp < right.stamp || (left.stamp == right.stamp && left.id < right.id));
        }
    };

    // output
    sensor_msgs::JointState jointStateOut;
    r2_msgs::JointControlDataArray   jointControlOut;

    // trajectory helpers
    typedef TrapezoidalVelocityJointTrajectoryFactory jointTrajFactory_type;
    typedef TrapezoidalVelocitySynchedCartesianTrajectoryFactory cartTrajFactory_type;
    boost::shared_ptr<jointTrajFactory_type> jointTrajFactory;
    boost::shared_ptr<cartTrajFactory_type> cartTrajFactory;
    RosMsgJointTrajectoryFactory rosJointTrajFactory;
    RosMsgCartesianTrajectoryFactory rosCartTrajFactory;
    RosMsgTreeIkTrajectoryFactory rosTreeIkTrajFactory;
    RosMsgForceTrajectoryFactory rosForceTrajFactory;
    r2_msgs::ControllerJointSettings jointSettings;
    boost::shared_ptr<JointNamePositionLimiter> positionLimiter;

    // other helpers
    std::map<std::string, unsigned int> jointIndexMap;
    boost::shared_ptr<MobileTreeIk> treeIkPtr;
    boost::shared_ptr<Cartesian_HybCntrl> forceController;
    std::map<std::string, bool> controllableJoints;
    std::map<std::string, double> jointInertiaMap;
    std::map<std::string, KDL::Wrench> forceSensorMap;
    std::vector<std::string> actualFrameNames;
    std::map<std::string, KDL::FrameVel> actualFrameVelMap;

    RosMsgToolFrameManager toolFrameManager;

    // keep track of goals
    typedef boost::ptr_map<actionlib_msgs::GoalID, JointTrajectoryFollower, GoalIdCompare> GoalFollowerMap_type;
    GoalFollowerMap_type goalFollowerMap;
    typedef std::map<actionlib_msgs::GoalID, actionlib_msgs::GoalStatus, GoalIdCompare> GoalStatusMap_type;
    GoalStatusMap_type goalStatusMap;
    typedef std::map<actionlib_msgs::GoalID, std::vector<r2_msgs::ReplanType>, GoalIdCompare> GoalReplanMap_type;
    GoalReplanMap_type goalReplanMap;

    void setupJointState(const r2_msgs::JointCommand& jointCommand);
    void setupJointControl(const r2_msgs::JointControlDataArray& jointControl);
};

#endif