6 #include <visp3/core/vpConfig.h> 8 #ifdef VISP_HAVE_FRANKA 10 #include <franka/exception.h> 11 #include <franka/robot.h> 25 int main(
int argc,
char **argv)
28 std::cerr <<
"Usage: ./generate_cartesian_velocity_motion <robot-hostname>" << std::endl;
32 franka::Robot robot(argv[1]);
36 robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
39 std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
40 std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
41 std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
42 std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
43 std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
44 std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
45 std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
46 std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
47 robot.setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
48 lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
49 lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
50 lower_force_thresholds_nominal, upper_force_thresholds_nominal);
52 double time_max = 4.0;
54 double angle = M_PI / 4.0;
56 robot.control([=, &time](
const franka::RobotState &, franka::Duration time_step) -> franka::CartesianVelocities {
57 time += time_step.toSec();
59 double cycle = std::floor(pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
60 double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));
61 double v_x = std::cos(angle) * v;
62 double v_z = -std::sin(angle) * v;
64 franka::CartesianVelocities output = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}};
65 if (time >= 2 * time_max) {
66 std::cout << std::endl <<
"Finished motion, shutting down example" << std::endl;
67 return franka::MotionFinished(output);
71 }
catch (
const franka::Exception &e) {
72 std::cout << e.what() << std::endl;
80 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }