Merge pull request #43 from fzi-forschungszentrum-informatik/devel

Simplify and stabilize the control loop

cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.hpp

@@ -114,6 +114,9 @@ template <class HardwareInterface>
 void CartesianComplianceController<HardwareInterface>::
 update(const ros::Time& time, const ros::Duration& period)
 {
+  // Synchronize the internal model and the real robot
+  Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_handles);
+
   // Control the robot motion in such a way that the resulting net force
   // vanishes. This internal control needs some simulation time steps.
   for (int i = 0; i < Base::m_iterations; ++i)
@@ -133,21 +136,6 @@ update(const ros::Time& time, const ros::Duration& period)
   Base::writeJointControlCmds();
 }
 
-template <>
-void CartesianComplianceController<hardware_interface::VelocityJointInterface>::
-update(const ros::Time& time, const ros::Duration& period)
-{
-  // Simulate only one step forward.
-  // The constant simulation time adds to solver stability.
-  ros::Duration internal_period(0.02);
-
-  ctrl::Vector6D error = computeComplianceError();
-
-  Base::computeJointControlCmds(error,internal_period);
-
-  Base::writeJointControlCmds();
-}
-
 template <class HardwareInterface>
 ctrl::Vector6D CartesianComplianceController<HardwareInterface>::
 computeComplianceError()

cartesian_controller_base/CMakeLists.txt

@@ -145,13 +145,11 @@ add_library(${PROJECT_NAME}
   include/cartesian_controller_base/Utility.h
   src/IKSolver.cpp
   include/cartesian_controller_base/IKSolver.h
-  include/cartesian_controller_base/IKSolver.hpp
 )
 
 add_library(ik_solvers
   src/IKSolver.cpp
   include/cartesian_controller_base/IKSolver.h
-  include/cartesian_controller_base/IKSolver.hpp
   src/ForwardDynamicsSolver.cpp
   include/cartesian_controller_base/ForwardDynamicsSolver.h
   src/JacobianTransposeSolver.cpp

cartesian_controller_base/include/cartesian_controller_base/IKSolver.h

@@ -125,6 +125,17 @@ class IKSolver
     //! Set initial joint configuration
     bool setStartState(const std::vector<hardware_interface::JointHandle>& joint_handles);
 
+    /**
+     * @brief Synchronize joint positions with the real robot
+     *
+     * Call this periodically in the controller's update() function.
+     * The internal model's joint velocity is not sychronized. Derived IK
+     * solvers should implement how to keep or reset those values.
+     *
+     * @param joint_handles Read handles to the joints.
+     */
+    void synchronizeJointPositions(const std::vector<hardware_interface::JointHandle>& joint_handles);
+
     /**
      * @brief Initialize the solver
      *
@@ -143,22 +154,10 @@ class IKSolver
     /**
      * @brief Update the robot kinematics of the solver
      *
-     * This template has two specializations for two distinct controller
-     * policies, depending on the hardware interface used:
-     *
-     * 1) PositionJointInterface: The solver's internal simulation is continued
-     * on each call without taking the real robot state into account.
-     *
-     * 2) VelocityJointInterface: The internal simulation is updated with the
-     * real robot state. On each call, the solver starts with its internal
-     * simulation in sync with the real robot.
-     *
-     * @tparam HardwareInterface
-     * @param joint_handles
+     * Call this periodically to update the internal simulation's forward
+     * kinematics.
      */
-    template <class HardwareInterface>
-    void updateKinematics(
-        const std::vector<hardware_interface::JointHandle>& joint_handles);
+    void updateKinematics();
 
   protected:
 
@@ -183,6 +182,7 @@ class IKSolver
     KDL::JntArray m_current_velocities;
     KDL::JntArray m_current_accelerations;
     KDL::JntArray m_last_positions;
+    KDL::JntArray m_last_velocities;
 
     // Joint limits
     KDL::JntArray m_upper_pos_limits;
@@ -200,6 +200,4 @@ class IKSolver
 
 } // namespace
 
-#include <cartesian_controller_base/IKSolver.hpp>
-
 #endif

cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h

@@ -93,11 +93,6 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
 
     virtual void starting(const ros::Time& time);
 
-    void pause(const ros::Time& time);
-
-    bool resume(const ros::Time& time);
-
-
   protected:
     /**
      * @brief Write joint control commands to the real hardware
@@ -160,11 +155,10 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
     std::string m_end_effector_link;
     std::string m_robot_base_link;
 
-    bool m_paused;
     int m_iterations;
+    std::vector<hardware_interface::JointHandle>      m_joint_handles;
 
   private:
-    std::vector<hardware_interface::JointHandle>      m_joint_handles;
     std::vector<std::string>                          m_joint_names;
     trajectory_msgs::JointTrajectoryPoint             m_simulated_joint_motion;
     SpatialPDController                              m_spatial_controller;

cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp

@@ -201,9 +201,6 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
 
   m_already_initialized = true;
 
-  // Start with normal ROS control behavior
-  m_paused = false;
-
   return true;
 }
 
@@ -213,37 +210,13 @@ starting(const ros::Time& time)
 {
   // Copy joint state to internal simulation
   m_ik_solver->setStartState(m_joint_handles);
-  m_ik_solver->updateKinematics<HardwareInterface>(m_joint_handles);
-}
-
-template <class HardwareInterface>
-void CartesianControllerBase<HardwareInterface>::
-pause(const ros::Time& time)
-{
-  m_paused = true;
-}
-
-template <class HardwareInterface>
-bool CartesianControllerBase<HardwareInterface>::
-resume(const ros::Time& time)
-{
-  m_paused = false;
-  return true;
+  m_ik_solver->updateKinematics();
 }
 
 template <>
 void CartesianControllerBase<hardware_interface::PositionJointInterface>::
 writeJointControlCmds()
 {
-  // Don't update position commands when paused.
-  // Note: CartesianMotionControllers don't take any feedback from the joint
-  // handles. These controllers will drift if the target frame they are
-  // following isn't also paused.
-  if (m_paused)
-  {
-    return;
-  }
-
   // Take position commands
   for (size_t i = 0; i < m_joint_handles.size(); ++i)
   {
@@ -255,15 +228,6 @@ template <>
 void CartesianControllerBase<hardware_interface::VelocityJointInterface>::
 writeJointControlCmds()
 {
-  // Don't update velocity commands when paused.
-  // Note: CartesianMotionControllers don't take any feedback from the joint
-  // handles. These controllers will drift if the target frame they are
-  // following isn't also paused.
-  if (m_paused)
-  {
-    return;
-  }
-
   // Take velocity commands
   for (size_t i = 0; i < m_joint_handles.size(); ++i)
   {
@@ -275,11 +239,6 @@ template <class HardwareInterface>
 void CartesianControllerBase<HardwareInterface>::
 computeJointControlCmds(const ctrl::Vector6D& error, const ros::Duration& period)
 {
-  if (m_paused)
-  {
-    return;
-  }
-
   // PD controlled system input
   m_cartesian_input = m_error_scale * m_spatial_controller(error,period);
 
@@ -288,8 +247,7 @@ computeJointControlCmds(const ctrl::Vector6D& error, const ros::Duration& period
       period,
       m_cartesian_input);
 
-  // Update according to control policy for next cycle
-  m_ik_solver->updateKinematics<HardwareInterface>(m_joint_handles);
+  m_ik_solver->updateKinematics();
 }
 
 template <class HardwareInterface>

cartesian_controller_base/src/DampedLeastSquaresSolver.cpp

@@ -115,6 +115,9 @@ namespace cartesian_controller_base{
     }
     control_cmd.time_from_start = period; // valid for this duration
 
+    // Update for the next cycle
+    m_last_positions = m_current_positions;
+
     return control_cmd;
   }
 

cartesian_controller_base/src/ForwardDynamicsSolver.cpp

@@ -102,11 +102,11 @@ namespace cartesian_controller_base{
     // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
     m_current_accelerations.data = m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force;
 
-    // Integrate once, starting with zero motion
-    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.toSec();
-
-    // Integrate twice, starting with zero motion
-    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();
+    // Numerical time integration with the Euler forward method
+    m_current_positions.data = m_last_positions.data + m_last_velocities.data * period.toSec();
+    m_current_velocities.data = m_last_velocities.data + m_current_accelerations.data * period.toSec();
+    m_current_velocities.data *= 0.9;  // 10 % global damping against unwanted null space motion.
+                                       // Will cause exponential slow-down without input.
 
     // Make sure positions stay in allowed margins
     applyJointLimits();
@@ -124,6 +124,10 @@ namespace cartesian_controller_base{
     }
     control_cmd.time_from_start = period; // valid for this duration
 
+    // Update for the next cycle
+    m_last_positions = m_current_positions;
+    m_last_velocities = m_current_velocities;
+
     return control_cmd;
   }
 

cartesian_controller_base/src/IKSolver.cpp

@@ -87,12 +87,24 @@ namespace cartesian_controller_base{
       m_current_positions(i)      = joint_handles[i].getPosition();
       m_current_velocities(i)     = joint_handles[i].getVelocity();
       m_current_accelerations(i)  = 0.0;
+
       m_last_positions(i)         = m_current_positions(i);
+      m_last_velocities(i)        = m_current_velocities(i);
     }
     return true;
   }
 
 
+  void IKSolver::synchronizeJointPositions(const std::vector<hardware_interface::JointHandle>& joint_handles)
+  {
+    for (size_t i = 0; i < joint_handles.size(); ++i)
+    {
+      m_current_positions(i) = joint_handles[i].getPosition();
+      m_last_positions(i)    = m_current_positions(i);
+    }
+  }
+
+
   bool IKSolver::init(ros::NodeHandle& nh,
                       const KDL::Chain& chain,
                       const KDL::JntArray& upper_pos_limits,
@@ -105,6 +117,7 @@ namespace cartesian_controller_base{
     m_current_velocities.data    = ctrl::VectorND::Zero(m_number_joints);
     m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints);
     m_last_positions.data        = ctrl::VectorND::Zero(m_number_joints);
+    m_last_velocities.data       = ctrl::VectorND::Zero(m_number_joints);
     m_upper_pos_limits           = upper_pos_limits;
     m_lower_pos_limits           = lower_pos_limits;
 
@@ -115,6 +128,22 @@ namespace cartesian_controller_base{
     return true;
   }
 
+  void IKSolver::updateKinematics()
+  {
+    // Pose w. r. t. base
+    m_fk_pos_solver->JntToCart(m_current_positions,m_end_effector_pose);
+
+    // Absolute velocity w. r. t. base
+    KDL::FrameVel vel;
+    m_fk_vel_solver->JntToCart(KDL::JntArrayVel(m_current_positions,m_current_velocities),vel);
+    m_end_effector_vel[0] = vel.deriv().vel.x();
+    m_end_effector_vel[1] = vel.deriv().vel.y();
+    m_end_effector_vel[2] = vel.deriv().vel.z();
+    m_end_effector_vel[3] = vel.deriv().rot.x();
+    m_end_effector_vel[4] = vel.deriv().rot.y();
+    m_end_effector_vel[5] = vel.deriv().rot.z();
+  }
+
   void IKSolver::applyJointLimits()
   {
     for (int i = 0; i < m_number_joints; ++i)