overload admittance->update() to account for both joint_state and pos…

…e_stamped goals

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -111,16 +111,32 @@ class AdmittanceRule
  controller_interface::return_type reset(const size_t num_joints);
 
  /**
- * Calculate all transforms needed for admittance control using the loader kinematics plugin. If
+ * Calculate transforms needed for admittance control using the loader kinematics plugin. If
  * the transform does not exist in the kinematics model, then TF will be used for lookup. The
  * return value is true if all transformation are calculated without an error 
  * \param[in] current_joint_state current joint state of the robot 
+ * \param[out] success true if no calls to the kinematics interface fail
+ */ 
+ bool get_current_state_transforms(
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state);
+ /**
+ * Calculate ft_sensor_link -> base_link transform needed for admittance control using the loader kinematics plugin. If
+ * the transform does not exist in the kinematics model, then TF will be used for lookup. The
+ * return value is true if all transformation are calculated without an error 
  * \param[in] reference_pose input ft sensor reference pose
  * \param[out] success true if no calls to the kinematics interface fail
  */
- bool get_all_transforms(
- const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+ bool get_reference_state_transforms(
  const geometry_msgs::msg::PoseStamped & reference_pose);
+ /**
+ * Calculate ft_sensor_link -> base_link transform needed for admittance control using the loader kinematics plugin. If
+ * the transform does not exist in the kinematics model, then TF will be used for lookup. The
+ * return value is true if all transformation are calculated without an error 
+ * \param[in] reference_joint_state input reference joint state
+ * \param[out] success true if no calls to the kinematics interface fail
+ */
+ bool get_reference_state_transforms(
+ const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);
 
  /**
  * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent
@@ -130,12 +146,12 @@ class AdmittanceRule
  void apply_parameters_update();
 
  /**
- * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and
+ * Calculate 'desired joint states' based on the 'measured force', 'reference goal pose', and
  * 'current_joint_state'.
  *
  * \param[in] current_joint_state current joint state of the robot
  * \param[in] measured_wrench most recent measured wrench from force torque sensor
- * \param[in] reference_pose input joint state reference
+ * \param[in] reference_pose input reference pose
  * \param[in] period time in seconds since last controller update
  * \param[out] desired_joint_state joint state reference after the admittance offset is applied to
  * the input reference
@@ -144,6 +160,23 @@ class AdmittanceRule
  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
  const geometry_msgs::msg::Wrench & measured_wrench,
  const geometry_msgs::msg::PoseStamped & reference_pose,
+ const rclcpp::Duration & period,
+ trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states);
+
+ /**
+ * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and
+ * 'current_joint_state'.
+ *
+ * \param[in] current_joint_state current joint state of the robot
+ * \param[in] measured_wrench most recent measured wrench from force torque sensor
+ * \param[in] reference_joint_state input joint state reference
+ * \param[in] period time in seconds since last controller update
+ * \param[out] desired_joint_state joint state reference after the admittance offset is applied to
+ * the input reference
+ */
+ controller_interface::return_type update(
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+ const geometry_msgs::msg::Wrench & measured_wrench,
  const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
  const rclcpp::Duration & period,
  trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states);
@@ -170,6 +203,15 @@ class AdmittanceRule
  admittance_controller::Params parameters_;
 
 protected:
+ /**
+ * Populate the AdmittanceState member using the current AdmittanceTransforms
+ * \param[in] current_joint_state
+ * \param[in] measured_wrench
+ */
+ void get_admittance_state_from_transforms(
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+ const geometry_msgs::msg::Wrench & measured_wrench);
+
  /**
  * Calculates the admittance rule from given the robot's current joint angles. The admittance
  * controller state input is updated with the new calculated values. A boolean value is returned

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -148,16 +148,11 @@ void AdmittanceRule::apply_parameters_update()
 }
 
 
-bool AdmittanceRule::get_all_transforms(
- const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
- const geometry_msgs::msg::PoseStamped & reference_pose)
+bool AdmittanceRule::get_current_state_transforms(
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state)
 {
- // get reference transforms 
- bool success=true; 
- tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_);
-
  // get transforms at current configuration
- success &= kinematics_->calculate_link_transform(
+ bool success = kinematics_->calculate_link_transform(
  current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_);
  success &= kinematics_->calculate_link_transform(
  current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_);
@@ -173,13 +168,26 @@ bool AdmittanceRule::get_all_transforms(
 
  return success;
 }
+bool AdmittanceRule::get_reference_state_transforms(
+ const geometry_msgs::msg::PoseStamped & reference_pose)
+{
+ tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_);
+ return true;
+}
+bool AdmittanceRule::get_reference_state_transforms(
+ const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state)
+{
+ bool success = kinematics_->calculate_link_transform(
+ reference_joint_state.positions, parameters_.ft_sensor.frame.id,
+ admittance_transforms_.ref_base_ft_);
+ return success;
+}
 
 // Update from reference joint states
 controller_interface::return_type AdmittanceRule::update(
  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
  const geometry_msgs::msg::Wrench & measured_wrench,
- const geometry_msgs::msg::PoseStamped & reference_pose,
- const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, 
+ const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
  const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
 {
  const double dt = period.seconds();
@@ -188,25 +196,9 @@ controller_interface::return_type AdmittanceRule::update(
  {
  apply_parameters_update();
  }
- bool success = get_all_transforms(current_joint_state, reference_pose);
- // apply filter and update wrench_world_ vector
- Eigen::Matrix<double, 3, 3> rot_world_sensor =
- admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation();
- Eigen::Matrix<double, 3, 3> rot_world_cog =
- admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation();
- process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog);
-
- // transform wrench_world_ into base frame
- admittance_state_.wrench_base.block<3, 1>(0, 0) =
- admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0);
- admittance_state_.wrench_base.block<3, 1>(3, 0) =
- admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0);
-
- // Compute admittance control law
- vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos);
- admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation();
- admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_;
- admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id;
+ bool success = get_current_state_transforms(current_joint_state);
+ success &= get_reference_state_transforms(reference_joint_state);
+ get_admittance_state_from_transforms(current_joint_state, measured_wrench);
  success &= calculate_admittance_rule(admittance_state_, dt);
 
  // if a failure occurred during any kinematics interface calls, return an error and don't
@@ -227,10 +219,69 @@ controller_interface::return_type AdmittanceRule::update(
  desired_joint_state.accelerations[i] =
  reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i];
  }
+ return controller_interface::return_type::OK;
+}
+
+controller_interface::return_type AdmittanceRule::update(
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+ const geometry_msgs::msg::Wrench & measured_wrench,
+ const geometry_msgs::msg::PoseStamped & reference_pose,
+ const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
+{
+ const double dt = period.seconds();
+
+ if (parameters_.enable_parameter_update_without_reactivation)
+ {
+ apply_parameters_update();
+ }
+ bool success = get_current_state_transforms(current_joint_state);
+ success &= get_reference_state_transforms(reference_pose);
+ get_admittance_state_from_transforms(current_joint_state, measured_wrench);
+ success &= calculate_admittance_rule(admittance_state_, dt);
+
+ // if a failure occurred during any kinematics interface calls, return an error and
+ // remain at current state
+ if (!success)
+ {
+ desired_joint_state = current_joint_state;
+ return controller_interface::return_type::ERROR;
+ }
+
+ // update joint desired joint state
+ for (size_t i = 0; i < num_joints_; ++i)
+ {
+ desired_joint_state.positions[i] = admittance_state_.joint_pos[i];
+ desired_joint_state.velocities[i] = admittance_state_.joint_vel[i];
+ desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i];
+ }
 
  return controller_interface::return_type::OK;
 }
 
+void AdmittanceRule::get_admittance_state_from_transforms(
+ const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
+ const geometry_msgs::msg::Wrench & measured_wrench)
+{
+ // apply filter and update wrench_world_ vector
+ Eigen::Matrix<double, 3, 3> rot_world_sensor =
+ admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation();
+ Eigen::Matrix<double, 3, 3> rot_world_cog =
+ admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation();
+ process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog);
+
+ // transform wrench_world_ into base frame
+ admittance_state_.wrench_base.block<3, 1>(0, 0) =
+ admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0);
+ admittance_state_.wrench_base.block<3, 1>(3, 0) =
+ admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0);
+
+ // Compute admittance control law
+ vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos);
+ admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation();
+ admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_;
+ admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id;
+}
+
 bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt)
 {
  // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness

admittance_controller/src/admittance_controller.cpp

@@ -427,7 +427,18 @@ controller_interface::return_type AdmittanceController::update_and_write_command
  read_state_from_hardware(joint_state_, ft_values_);
 
  // apply admittance control to reference to determine desired state
- admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, reference_, period, reference_admittance_);
+
+ /* some flag that determines whether we call the update() with goal_pose or with joint reference.
+ How can we know here whether the reference was updated from subscribers or from itnerfaces?
+ 
+ if(updated_from_subscribers)
+ admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_);
+ else
+ admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_);
+ 
+ */
+
+ admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_);
 
  // write calculated values to joint interfaces
  write_state_to_hardware(reference_admittance_);