IEEE Robotics & Automation Magazine - June 2022 - 58

establish a connection with the robot and read the initial
state with the r e a d O n c e function of the
franka::Robot object:
// connect to robot
franka::Robot robot(robot_ip);
// load the kinematics and dynamics model
franka::Model model = robot.loadModel();
franka::RobotState initial_state =
robot.readOnce();
Eigen::Map>
force_ext_initial(initial_state.O_F_
ext_hat_K. data());
force_error_integral.setZero();
// 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_initial
(initial_transform.linear ());
double time = 0.0;
Note that to also correct the slightest bias that might be present
in the external force estimation, we store the initial force
(before establishing contact) as a bias correction, and we initialize
the integral term in (4) to 0. The force estimation during
motion (especially during high accelerations) is less
accurate in comparison to the quasi-static case known from
previous literature. This is an inherent property of the implemented
disturbance observer. The static bias can further be
caused by imprecise vertical mounting of the robot base,
imprecise compensation of the end effector's mass and inertia
tensor, or a missing update of finger dynamics.
A callback function for our controller is defined as a
C++11 function as
// define callback for the torque control
loop
std::function
plug_in_control_callback =
[&] (const franka::RobotState& robot_state,
franka::Duration period) ->
franka::Torques {
// ... code for callback
}
This function, which receives the robot state and duration of
the cycle, will be called every millisecond until a
58 * IEEE ROBOTICS & AUTOMATION MAGAZINE * JUNE 2022
// update time
time += period.toSec();
// get state variables
std::array coriolis_array =
model.Coriolis(robot_state);
std::array jacobian_array =
model.zeroJacobian(franka::Frame::kEnd
Effector, robot_state);
// convert to Eigen
Eigen::Map> coriolis(coriolis_array.data());
Eigen::Map> Jacobian (jacobian_array.data());
Eigen::Map> force_ext (robot_state.O_F_ext
_hat_K.data ());
Eigen::Map> 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.
linear());
To implement (5), the orientation of the equilibrium pose
is computed as
Eigen::AngleAxisd angle_axis_wiggle_x;
angle_axis_wiggle_x.axis() << 1, 0, 0;
angle_axis_wiggle_x.angle() = sin(2.0
franka::MotionFinished signal is sent by the callback.
We can then have a look at the callback code that implements
(5) and (2). We first cast all the involved variables to [28] to
easily perform matrix products and quaternion manipulation.
*
M_PI * time * plug_in_params.wiggle_
frequency_x) * plug_in_params.wiggle_
amplitude_x;
Eigen::AngleAxisd angle_axis_wiggle_y;
angle_axis_wiggle_y.axis() << 0, 1, 0;
angle_axis_wiggle_y.angle() = sin(2.0
*
M_PI * time * plug_in_params.wiggle_
frequency_y) * plug_in_params.wiggle_
amplitude_y;
Eigen::Quaterniond wiggle_x(angle_axis
_wiggle_x);
Eigen::Quaterniond wiggle_y(angle_axis
_wiggle_y);
Eigen::Quaterniond orientation_d
(wiggle_y * (
wiggle_x * orientation_initial));
IEEE Robotics & Automation Magazine - June 2022

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2022
