fix adm control (#32) (#33)

* fix adm rule

* revert impedance rule to normal inverse() (instead of LLT-based inverses)

* increase alpha pinv

* check for singularities in impedance control

* prepare admittance for singularity checks

* add commented alternative method for acc estimation

* fix missing var in impedance rule

* change start position

* use "end_effector_frame" instead of control frame

(cherry picked from commit 5921f9a)

Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com>

Author: mergify[bot]

cartesian_vic_controller/config/example_admittance_controller_config.yaml

@@ -42,9 +42,9 @@ cartesian_impedance_controller:
       is_enabled: false
       name: ""
 
-    control:
+    end_effector_frame:
       frame:
-        id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+        id: interaction_point
         external: false # control frame exists within URDF kinematic chain
 
     fixed_world_frame:   # Gravity points down (neg. Z) in this frame (Usually: world or base_link)

cartesian_vic_controller/config/example_impedance_controller_config.yaml

@@ -41,7 +41,7 @@ cartesian_impedance_controller:
       is_enabled: false
       name: ""
 
-    control:
+    end_effector_frame:
       frame:
         id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
         external: false # control frame exists within URDF kinematic chain

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp

@@ -206,6 +206,9 @@ class CartesianVicRule
   /// Jacobian pre-allocation (used to compute cartesian inertia matrix)
   Eigen::Matrix<double, 6, Eigen::Dynamic> J_private_;
 
+  /// Jacobian pre-allocation (used to compute cartesian inertia matrix)
+  // Eigen::Matrix<double, 6, Eigen::Dynamic> J_dot_private_;
+
   /// Filtered external torques
   Eigen::VectorXd filtered_external_torques_;
 };

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp

@@ -127,12 +127,12 @@ class VicInputData
   bool activate_nullspace_control = false;
   bool activate_gravity_compensation = false;
 
-  /// Name of the robot base frame
+  /// Name of the control frame in which is expressed the cartesian pose/vel/acc/wrench reference
   std::string base_frame;
   /// Name of the compliance (i.e., vic) frame in which compliance parameters are specified
   std::string vic_frame;
-  /// Name of the control frame in which is expressed the cartesian pose/vel/acc/wrench reference
-  std::string control_frame;
+  /// Name of the controlled robot end-effector frame
+  std::string end_effector_frame;
   /// Name of the force/torque sensor frame in which is expressed the measured wrench
   std::string ft_sensor_frame;
 
@@ -236,23 +236,16 @@ class VicState
 /// Transforms between frames used in the Vic controller
 struct VicTransforms
 {
-  // transformation from force torque sensor frame to base link frame at reference joint angles
-  Eigen::Isometry3d ref_base_ft_;
-  // transformation from force torque sensor frame to base link frame at reference + vic
-  // offset joint angles
+  // transformation from fixed world frame to base link frame
+  Eigen::Isometry3d world_base_;
+  // transformation from force torque sensor frame to base link frame
   Eigen::Isometry3d base_ft_;
-  // transformation from control frame to base link frame at reference + vic offset joint
-  // angles
-  Eigen::Isometry3d base_control_;
+  // transformation from controlled end effector (EE) frame to base link frame
+  Eigen::Isometry3d base_end_effector_;
+  // transformation from vic frame to base link frame
   Eigen::Isometry3d base_vic_;
-  // transformation from end effector frame to base link frame at reference + vic offset
-  // joint angles
-  Eigen::Isometry3d base_tip_;
-  // transformation from center of gravity frame to base link frame at reference + vic offset
-  // joint angles
+  // transformation from CoG frame (i.e., post-sensor inertial frame) to base link frame
   Eigen::Isometry3d base_cog_;
-  // transformation from world frame to base link frame
-  Eigen::Isometry3d world_base_;
 };
 
 

cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp

@@ -58,7 +58,7 @@ class VanillaCartesianAdmittanceRule : public CartesianVicRule
   Eigen::Matrix<double, Eigen::Dynamic, 6> J_pinv_;
   Eigen::Matrix<double, 6, Eigen::Dynamic> J_dot_;
 
-  double alpha_pinv_ = 0.000005;
+  double alpha_pinv_ = 0.0005;
 
   // Nullspace solver
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;

cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp

@@ -63,7 +63,7 @@ class VanillaCartesianImpedanceRule : public CartesianVicRule
 
   Eigen::VectorXd raw_joint_command_effort_;
 
-  double alpha_pinv_ = 0.000005;
+  double alpha_pinv_ = 0.0005;
 
   // Nullspace solver
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;

cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml

@@ -111,11 +111,11 @@ cartesian_vic_controller:
       description: "Specifies the name of the external torque sensor in the robot description which will be used in the vic calculation."
     }
 
-  control:
+  end_effector_frame:
     frame:
       id: {
         type: string,
-        description: "Specifies the robot control frame in which the reference pose and velocities are expressed."
+        description: "Specifies the controlled robot end effector frame. Note that the pose and velocity are expressed w.r.t. the base frame (see dynamics plugin parameters)."
       }
 
   fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
@@ -149,7 +149,7 @@ cartesian_vic_controller:
     frame:
       id: {
         type: string,
-        description: "Specifies the control frame used for vic calculation."
+        description: "Specifies the frame in which the impedance matrices are expressed."
       }
     plugin_name: {
       type: string,

cartesian_vic_controller/src/cartesian_vic_rule.cpp

@@ -94,6 +94,9 @@ CartesianVicRule::configure(
   J_private_ = \
     Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(6, num_joints);
 
+  // J_dot_private_ = \
+  //  Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(6, num_joints);
+
   return controller_interface::return_type::OK;
 }
 
@@ -191,7 +194,7 @@ void CartesianVicRule::apply_parameters_update()
     parameters_.vic.activate_gravity_compensation;
 
   vic_state_.input_data.vic_frame = parameters_.vic.frame.id;
-  vic_state_.input_data.control_frame = parameters_.control.frame.id;
+  vic_state_.input_data.end_effector_frame = parameters_.end_effector_frame.frame.id;
   vic_state_.input_data.ft_sensor_frame = parameters_.ft_sensor.frame.id;
 
   if (!use_streamed_interaction_parameters_) {
@@ -318,7 +321,7 @@ CartesianVicRule::update_compliant_frame_trajectory(
   // Update reference compliant frames
   bool success = true;
   for (unsigned int i = 0; i < N; i++) {
-    // TODO(tpoignonec): Check the frame is correct (i.e., control w.r.t. base)!
+    // TODO(tpoignonec): Check the frame is correct (i.e., end-effector w.r.t. base)!
     success &= \
       vic_state_.input_data.reference_compliant_frames.fill_desired_robot_state_from_msg(
       i,
@@ -449,7 +452,7 @@ CartesianVicRule::update_input_data(
   );
   success &= dynamics_->calculate_jacobian(
     vic_state_.input_data.joint_state_position,
-    vic_state_.input_data.control_frame,
+    vic_state_.input_data.end_effector_frame,
     J_private_
   );
   vic_state_.input_data.natural_cartesian_inertia = (J_private_ * \
@@ -571,6 +574,7 @@ bool CartesianVicRule::update_kinematics(
   }
 
   // Update current robot joint velocity
+  // auto previous_joint_velocity = vic_state_.input_data.joint_state_velocity;
   double cutoff_jnt_velocity = parameters_.filters.state_velocity_filter_cuttoff_freq;
   if (dt > 0 && cutoff_jnt_velocity > 0.0) {
     double jnt_velocity_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_jnt_velocity);
@@ -589,15 +593,15 @@ bool CartesianVicRule::update_kinematics(
   // Update current cartesian pose and velocity from robot joint states
   success &= dynamics_->calculate_link_transform(
     vic_state_.input_data.joint_state_position,
-    vic_state_.input_data.control_frame,
+    vic_state_.input_data.end_effector_frame,
     vic_state_.input_data.robot_current_pose
   );
 
   auto last_robot_cartesian_velocity = vic_state_.input_data.robot_current_velocity;
   success = dynamics_->convert_joint_deltas_to_cartesian_deltas(
     vic_state_.input_data.joint_state_position,
     vic_state_.input_data.joint_state_velocity,
-    vic_state_.input_data.control_frame,
+    vic_state_.input_data.end_effector_frame,
     vic_state_.input_data.robot_current_velocity
   );
 
@@ -606,6 +610,17 @@ bool CartesianVicRule::update_kinematics(
   if (dt > 0) {
     auto raw_acc = \
       (vic_state_.input_data.robot_current_velocity - last_robot_cartesian_velocity) / dt;
+    /*
+    auto joint_acc = (vic_state_.input_data.joint_state_velocity - previous_joint_velocity) / dt;
+    success &= dynamics_->calculate_jacobian_derivative(
+      vic_state_.input_data.joint_state_position,
+      vic_state_.input_data.joint_state_velocity,
+      vic_state_.input_data.end_effector_frame,
+      J_dot_private_
+    );
+    auto raw_acc = J_private_ * joint_acc + J_dot_private_ *
+      vic_state_.input_data.joint_state_velocity;
+    */
     if (cutoff_acceleration > 0.0) {
       double cutoff_acceleration = 30.0;  // Hz
       double acceleration_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_acceleration);
@@ -635,12 +650,6 @@ bool CartesianVicRule::update_kinematics(
     // In theory, there is no reason to use it if no wrench is available, but you never know...
     vic_transforms_.base_ft_.setIdentity();
   }
-
-  success &= dynamics_->calculate_link_transform(
-    vic_state_.input_data.joint_state_position,
-    parameters_.dynamics.tip,
-    vic_transforms_.base_tip_
-  );
   success &= dynamics_->calculate_link_transform(
     vic_state_.input_data.joint_state_position,
     parameters_.fixed_world_frame.frame.id,
@@ -653,12 +662,12 @@ bool CartesianVicRule::update_kinematics(
   );
   success &= dynamics_->calculate_link_transform(
     vic_state_.input_data.joint_state_position,
-    parameters_.control.frame.id,
-    vic_transforms_.base_control_
+    parameters_.end_effector_frame.frame.id,
+    vic_transforms_.base_end_effector_
   );
   success &= dynamics_->calculate_link_transform(
     vic_state_.input_data.joint_state_position,
-    parameters_.control.frame.id,
+    parameters_.vic.frame.id,
     vic_transforms_.base_vic_
   );
   return true;
@@ -694,7 +703,7 @@ bool CartesianVicRule::process_wrench_measurements(
   new_wrench_world.block<3, 1>(0, 1) -= (rot_world_cog * cog_pos).cross(end_effector_weight);
 
   /*
-  // Wrench at interaction point (e.g., assumed to be control frame
+  // Wrench at interaction point (assumed to be end-effector frame)
   F_ext.block<3, 1>(0, 0) = rot_base_control.transpose() * F_ext_base.block<3, 1>(0, 0);
   // Evaluate torques at new interaction point
   F_ext.block<3, 1>(3, 0) = rot_base_control.transpose() * (

cartesian_vic_controller/src/cartesian_vic_state.cpp

@@ -54,6 +54,8 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg
       success = false;
   }
 
+  // Fill frame names
+
   // Fill desired compliance
   auto desired_frame_0 = \
     input_data.reference_compliant_frames.get_compliant_frame(0);