9 #include <visp3/core/vpConfig.h> 11 #ifdef VISP_HAVE_FRANKA 13 #include <franka/exception.h> 14 #include <franka/robot.h> 26 template <
class T,
size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
29 std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream,
","));
30 std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
35 int main(
int argc,
char **argv)
38 std::cerr <<
"Usage: " << argv[0] <<
" <robot-hostname> <trajectory-csv> <output>" << std::endl;
42 std::cout <<
"Loading csv trajectory" << std::endl;
43 std::fstream csv_file_stream;
44 csv_file_stream.open(argv[2], std::fstream::in);
46 std::vector<std::array<double, 7> > samples;
47 while (csv_file_stream) {
48 std::array<double, 7> q;
50 for (
int i = 0; i < 7; i++) {
51 csv_file_stream >> q[i] >> delimiter;
55 std::cout <<
"Read " << samples.size() <<
" samples" << std::endl;
57 std::vector<franka::RobotState> states;
59 franka::Robot robot(argv[1]);
63 robot.setCollisionBehavior(
64 {{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}},
65 {{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}},
66 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
67 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
70 robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
73 robot.control([&](
const franka::RobotState &robot_state, franka::Duration time_step) -> franka::JointPositions {
74 states.push_back(robot_state);
76 index += time_step.toMSec();
78 if (index >= samples.size() - 1) {
79 return franka::MotionFinished(franka::JointPositions(samples.back()));
81 return samples[index];
83 }
catch (
const franka::ControlException &e) {
84 std::cout << e.what() << std::endl;
85 }
catch (
const franka::Exception &e) {
86 std::cout << e.what() << std::endl;
90 std::cout <<
"Logging results to file: " << argv[3] << std::endl;
91 std::fstream output_stream;
92 output_stream.open(argv[3], std::fstream::out);
93 for (
size_t s = 0; s < states.size(); s++) {
94 output_stream <<
"Sample: #" << s << std::endl;
95 output_stream <<
"q_d: \t" << samples[s] << std::endl;
96 output_stream <<
"Robot state:" << std::endl;
97 output_stream <<
"q: \t" << states[s].q << std::endl;
98 output_stream <<
"q_d: \t" << states[s].q_d << std::endl;
99 output_stream <<
"dq: \t" << states[s].dq << std::endl;
100 output_stream <<
"______" << std::endl;
107 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }