How to combine compliance and cartesian pose motion control?

asked 2018-10-02 16:13:39 -0600

chloe gravatar image

updated 2018-10-02 16:20:18 -0600

Hello,

I am working with a Franka Emika Panda using ROS Kinetic and am fairly new to C++ and ROS. I am attempting to generate a cartesian motion of the end-effector that will be compliant with forces (so for example if it is commanded to move in the x direction, and you push on it in the y direction, it will move in y according to the force but also continue moving in x) . My strategy thus far has been to combine the cartesian_pose_example_controller.cpp (which moves the end effector in a quarter circle) with the cartesian_impedance_example_controller.cpp into a new controller: cartesian_impedance_position_controller.cpp. I got to the point where when I launch the following code with the control node, the cartesian impedance works, but it does not move in the quarter circle. Any thoughts on how to make this work or other strategies to try would be greatly appreciated! I apologize that the code is so long, let me know if I can remove or add information to be more helpful.

#include <franka_example_controllers/cartesian_impedance_position_controller.h>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>

#include <controller_interface/controller_base.h>
#include <franka/robot_state.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>

#include "pseudo_inversion.h"

namespace franka_example_controllers {

bool CartesianImpedancePositionController::init (hardware_interface::RobotHW* robot_hw,
                                               ros::NodeHandle& node_handle) {

//CARTPOS
auto cartesian_pose_interface_ = robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
  if (cartesian_pose_interface_ == nullptr) {
    ROS_ERROR(
        "CartesianPoseExampleController: Could not get Cartesian Pose "
        "interface from hardware");
    return false;
}
//ENDCARTPOS

  std::vector<double> cartesian_stiffness_vector;
  std::vector<double> cartesian_damping_vector;

sub_equilibrium_pose_ = node_handle.subscribe(
      "/equilibrium_pose", 20, &CartesianImpedancePositionController::equilibriumPoseCallback, this,
      ros::TransportHints().reliable().tcpNoDelay());

  std::string arm_id;
  if (!node_handle.getParam("arm_id", arm_id)) {
    ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
    return false;
  }
  std::vector<std::string> joint_names;
  if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
    ROS_ERROR(
        "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
        "aborting controller init!");
    return false;
  }

  auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
  if (model_interface == nullptr) {
    ROS_ERROR_STREAM(
        "CartesianImpedanceExampleController: Error getting model interface from hardware");
    return false;
  }

//CARTPOS
try {
    cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
        cartesian_pose_interface_->getHandle(arm_id + "_robot"));
  } catch (const hardware_interface::HardwareInterfaceException& e) {
    ROS_ERROR_STREAM(
        "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
    return false;
  }

auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
  if (state_interface == nullptr) {
    ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
    return false;
  }

  try {
    auto state_handle = state_interface->getHandle(arm_id + "_robot");

    std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    for (size_t i = 0; i < q_start.size(); i++) {
      if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
        ROS_ERROR_STREAM(
            "CartesianPoseExampleController: Robot is not in the expected starting position for "
            "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
            "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
        return false;
      }
    }
  } catch (const hardware_interface::HardwareInterfaceException& e) {
    ROS_ERROR_STREAM(
        "CartesianPoseExampleController: Exception getting state handle: " << e.what());
    return false;
  }
//ENDCARTPOS

  try {
    model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
        model_interface->getHandle(arm_id + "_model"));
  } catch (hardware_interface::HardwareInterfaceException& ex) {
    ROS_ERROR_STREAM(
        "CartesianImpedanceExampleController: Exception getting model handle from interface: "
        << ex.what());
    return false;
  }

  try {
    state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
        state_interface->getHandle(arm_id + "_robot"));
  } catch (hardware_interface::HardwareInterfaceException& ex) {
    ROS_ERROR_STREAM(
        "CartesianImpedanceExampleController: Exception getting state handle ...
(more)
edit retag flag offensive close merge delete

Comments

I could use help specifically with what kinds of controllers to use/combine and how to do that, or what ros topics the two types of commands should be sent to (effort and pose?) Should they be published to the same topic or separate topics?

chloe gravatar imagechloe ( 2018-10-04 08:52:40 -0600 )edit

I'm having a hunch that force-position won't work, and I need to do force-velocity? I think compliant control requires control of power transfer? I'm not sure how to go about that.

chloe gravatar imagechloe ( 2018-10-09 13:37:34 -0600 )edit