libfranka  0.14.1
FCI C++ API
generate_consecutive_motions.cpp

An example showing how to execute consecutive motions with error recovery.

Warning
Before executing this example, make sure there is enough space in front and to the side of the robot.
// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <cmath>
#include <iostream>
#include <franka/robot.h>
int main(int argc, char** argv) {
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
return -1;
}
try {
franka::Robot robot(argv[1]);
// First move the robot to a suitable joint configuration
std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
MotionGenerator motion_generator(0.5, q_goal);
std::cout << "WARNING: This example will move the robot! "
<< "Please make sure to have the user stop button at hand!" << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
robot.control(motion_generator);
std::cout << "Finished moving to initial joint configuration." << std::endl;
// Set additional parameters always before the control loop, NEVER in the control loop!
// Set collision behavior.
{{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}},
{{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}},
{{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}},
{{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}});
for (size_t i = 0; i < 5; i++) {
std::cout << "Executing motion." << std::endl;
try {
double time_max = 4.0;
double omega_max = 0.2;
double time = 0.0;
robot.control([=, &time](const franka::RobotState&,
time += period.toSec();
double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
franka::JointVelocities velocities = {{0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}};
if (time >= 2 * time_max) {
std::cout << std::endl << "Finished motion." << std::endl;
return franka::MotionFinished(velocities);
}
return velocities;
});
} catch (const franka::ControlException& e) {
std::cout << e.what() << std::endl;
std::cout << "Running error recovery..." << std::endl;
}
}
} catch (const franka::Exception& e) {
std::cout << e.what() << std::endl;
return -1;
}
std::cout << "Finished." << std::endl;
return 0;
}
franka::Robot::automaticErrorRecovery
void automaticErrorRecovery()
Runs automatic error recovery on the robot.
franka::JointVelocities
Stores values for joint velocity motion generation.
Definition: control_types.h:99
MotionGenerator
An example showing how to generate a joint pose motion to a goal position.
Definition: examples_common.h:31
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
franka::Duration::toSec
double toSec() const noexcept
Returns the stored duration in .
franka::MotionFinished
CartesianVelocities MotionFinished(CartesianVelocities command) noexcept
Helper method to indicate that a motion should stop after processing the given command.
Definition: control_types.h:353
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::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
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::ControlException
ControlException is thrown if an error occurs during motion generation or torque control.
Definition: exception.h:73
franka::Robot
Maintains a network connection to the robot, provides the current robot state, gives access to the mo...
Definition: robot.h:68