libfranka 0.14.1
FCI C++ API
generate_elbow_motion.cpp

An example showing how to move the robot's elbow.

An example showing how to move the robot's elbow.

Warning
Before executing this example, make sure that the elbow has enough space to move.
// 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.
{{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
{{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
{{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
{{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
std::array<double, 16> initial_pose;
std::array<double, 2> initial_elbow;
double time = 0.0;
robot.control(
[&time, &initial_pose, &initial_elbow](const franka::RobotState& robot_state,
time += period.toSec();
if (time == 0.0) {
initial_pose = robot_state.O_T_EE_c;
initial_elbow = robot_state.elbow_c;
}
double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * time));
auto elbow = initial_elbow;
elbow[0] += angle;
if (time >= 10.0) {
std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
return franka::MotionFinished({initial_pose, elbow});
}
return {initial_pose, elbow};
});
} catch (const franka::Exception& e) {
std::cout << e.what() << std::endl;
return -1;
}
return 0;
}
An example showing how to generate a joint pose motion to a goal position.
Definition: examples_common.h:31
Stores values for Cartesian pose motion generation.
Definition: control_types.h:127
Represents a duration with millisecond resolution.
Definition: duration.h:19
double toSec() const noexcept
Returns the stored duration in .
Maintains a network connection to the robot, provides the current robot state, gives access to the mo...
Definition: robot.h:68
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.
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.
Contains common types and functions for the examples.
void setDefaultBehavior(franka::Robot &robot)
Sets a default collision behavior, joint impedance and Cartesian impedance.
Definition: examples_common.cpp:12
Contains exception definitions.
Contains the franka::Robot type.
Base class for all exceptions used by libfranka.
Definition: exception.h:20
Describes the robot state.
Definition: robot_state.h:34
std::array< double, 2 > elbow_c
Commanded elbow configuration.
Definition: robot_state.h:191
std::array< double, 16 > O_T_EE_c
Last commanded end effector pose of motion generation in base frame.
Definition: robot_state.h:342