libfranka  0.14.1
FCI C++ API
cartesian_impedance_control.cpp

An example showing a simple cartesian impedance controller without inertia shaping that renders a spring damper system where the equilibrium is the initial configuration.After starting the controller try to push the robot around and try different stiffness levels.

Warning
collision thresholds are set to high values. Make sure you have the user stop at hand!
// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include <Eigen/Dense>
#include <franka/model.h>
#include <franka/robot.h>
int main(int argc, char** argv) {
// Check whether the required arguments were passed
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
return -1;
}
// Compliance parameters
const double translational_stiffness{150.0};
const double rotational_stiffness{10.0};
Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
stiffness.setZero();
stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
damping.setZero();
damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
Eigen::MatrixXd::Identity(3, 3);
damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
Eigen::MatrixXd::Identity(3, 3);
try {
// connect to robot
franka::Robot robot(argv[1]);
// load the kinematics and dynamics model
franka::Model model = robot.loadModel();
franka::RobotState initial_state = robot.readOnce();
// equilibrium point is the initial position
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
Eigen::Vector3d position_d(initial_transform.translation());
Eigen::Quaterniond orientation_d(initial_transform.rotation());
// set collision behavior
robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
// define callback for the torque control loop
impedance_control_callback = [&](const franka::RobotState& robot_state,
franka::Duration /*duration*/) -> franka::Torques {
// get state variables
std::array<double, 7> coriolis_array = model.coriolis(robot_state);
std::array<double, 42> jacobian_array =
model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
// convert to Eigen
Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
Eigen::Vector3d position(transform.translation());
Eigen::Quaterniond orientation(transform.rotation());
// compute error to desired equilibrium pose
// position error
Eigen::Matrix<double, 6, 1> error;
error.head(3) << position - position_d;
// orientation error
// "difference" quaternion
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
orientation.coeffs() << -orientation.coeffs();
}
// "difference" quaternion
Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
// Transform to base frame
error.tail(3) << -transform.rotation() * error.tail(3);
// compute control
Eigen::VectorXd tau_task(7), tau_d(7);
// Spring damper system with damping ratio=1
tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
tau_d << tau_task + coriolis;
std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
return tau_d_array;
};
// start real-time control loop
std::cout << "WARNING: Collision thresholds are set to high values. "
<< "Make sure you have the user stop at hand!" << std::endl
<< "After starting try to push the robot and see how it reacts." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
robot.control(impedance_control_callback);
} catch (const franka::Exception& ex) {
// print exception
std::cout << ex.what() << std::endl;
}
return 0;
}
franka::Robot::setCollisionBehavior
void setCollisionBehavior(const std::array< double, 7 > &lower_torque_thresholds_acceleration, const std::array< double, 7 > &upper_torque_thresholds_acceleration, const std::array< double, 7 > &lower_torque_thresholds_nominal, const std::array< double, 7 > &upper_torque_thresholds_nominal, const std::array< double, 6 > &lower_force_thresholds_acceleration, const std::array< double, 6 > &upper_force_thresholds_acceleration, const std::array< double, 6 > &lower_force_thresholds_nominal, const std::array< double, 6 > &upper_force_thresholds_nominal)
Changes the collision behavior.
robot.h
model.h
franka::Robot::readOnce
virtual RobotState readOnce()
Waits for a robot state update and returns it.
franka::Model
Calculates poses of joints and dynamic properties of the robot.
Definition: model.h:52
franka::RobotState::q
std::array< double, 7 > q
Measured joint position.
Definition: robot_state.h:233
franka::Robot::loadModel
Model loadModel()
Loads the model library from the robot.
franka::Robot::control
void control(std::function< Torques(const RobotState &, franka::Duration)> control_callback, bool limit_rate=false, double cutoff_frequency=kDefaultCutoffFrequency)
Starts a control loop for sending joint-level torque commands.
franka::Torques
Stores joint-level torque commands without gravity and friction.
Definition: control_types.h:45
franka::Model::coriolis
std::array< double, 7 > coriolis(const franka::RobotState &robot_state) const noexcept
Calculates the Coriolis force vector (state-space equation): , in .
franka::Exception
Base class for all exceptions used by libfranka.
Definition: exception.h:20
setDefaultBehavior
void setDefaultBehavior(franka::Robot &robot)
Sets a default collision behavior, joint impedance and Cartesian impedance.
Definition: examples_common.cpp:12
duration.h
franka::RobotState::O_T_EE
std::array< double, 16 > O_T_EE
Measured end effector pose in base frame.
Definition: robot_state.h:40
franka::Model::zeroJacobian
std::array< double, 42 > zeroJacobian(Frame frame, const franka::RobotState &robot_state) const
Gets the 6x7 Jacobian for the given joint relative to the base frame.
franka::Duration
Represents a duration with millisecond resolution.
Definition: duration.h:19
examples_common.h
franka::RobotState
Describes the robot state.
Definition: robot_state.h:34
exception.h
franka::RobotState::dq
std::array< double, 7 > dq
Measured joint velocity.
Definition: robot_state.h:245
franka::Robot
Maintains a network connection to the robot, provides the current robot state, gives access to the mo...
Definition: robot.h:68