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_pose_motion <robot-hostname>" << std::endl;
33 franka::Robot robot(argv[1]);
37 robot.setCollisionBehavior(
38 {{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}},
39 {{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}},
40 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
41 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
43 auto initial_pose = robot.readOnce().O_T_EE_d;
46 robot.control([=, &time](
const franka::RobotState &, franka::Duration time_step) -> franka::CartesianPose {
47 time += time_step.toSec();
49 double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time));
50 double delta_x = radius * std::sin(angle);
51 double delta_z = radius * (std::cos(angle) - 1);
53 std::array<double, 16> new_pose = initial_pose;
54 new_pose[12] += delta_x;
55 new_pose[14] += delta_z;
58 std::cout << std::endl <<
"Finished motion, shutting down example" << std::endl;
59 return franka::MotionFinished(new_pose);
63 }
catch (
const franka::Exception &e) {
64 std::cout << e.what() << std::endl;
72 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }