8 #include <visp3/core/vpConfig.h> 10 #ifdef VISP_HAVE_FRANKA 12 #include <franka/exception.h> 13 #include <franka/robot.h> 34 Controller(
size_t dq_filter_size,
const std::array<double, 7> &K_P,
35 const std::array<double, 7> &K_D)
36 : dq_current_filter_position_(0), dq_filter_size_(dq_filter_size), K_P_(K_P), K_D_(K_D)
38 std::fill(dq_d_.begin(), dq_d_.end(), 0);
39 dq_buffer_.reset(
new double[dq_filter_size_ * 7]);
40 std::fill(&dq_buffer_.get()[0], &dq_buffer_.get()[dq_filter_size_ * 7], 0);
43 inline franka::Torques step(
const franka::RobotState &state)
45 updateDQFilter(state);
47 std::array<double, 7> tau_J_d;
48 for (
size_t i = 0; i < 7; i++) {
49 tau_J_d[i] = K_P_[i] * (state.q_d[i] - state.q[i]) + K_D_[i] * (dq_d_[i] - getDQFiltered(i));
54 void updateDQFilter(
const franka::RobotState &state)
56 for (
size_t i = 0; i < 7; i++) {
57 dq_buffer_.get()[dq_current_filter_position_ * 7 + i] = state.dq[i];
59 dq_current_filter_position_ = (dq_current_filter_position_ + 1) % dq_filter_size_;
62 double getDQFiltered(
size_t index)
const 65 for (
size_t i = index; i < 7 * dq_filter_size_; i += 7) {
66 value += dq_buffer_.get()[i];
68 return value / dq_filter_size_;
72 size_t dq_current_filter_position_;
73 size_t dq_filter_size_;
75 const std::array<double, 7> K_P_;
76 const std::array<double, 7> K_D_;
78 std::array<double, 7> dq_d_;
79 std::unique_ptr<double[]> dq_buffer_;
82 std::vector<double> generateTrajectory(
double a_max)
86 std::vector<double> trajectory;
87 constexpr
double kTimeStep = 0.001;
88 constexpr
double kAccelerationTime = 1;
89 constexpr
double kConstantVelocityTime = 1;
95 while (t < (2 * kAccelerationTime + kConstantVelocityTime)) {
96 if (t <= kAccelerationTime) {
97 a = pow(sin(t * M_PI / kAccelerationTime), 2) * a_max;
98 }
else if (t <= (kAccelerationTime + kConstantVelocityTime)) {
101 const double deceleration_time =
102 (kAccelerationTime + kConstantVelocityTime) - t;
103 a = -pow(sin(deceleration_time * M_PI / kAccelerationTime), 2) * a_max;
107 trajectory.push_back(v);
114 int main(
int argc,
char **argv)
117 std::cerr <<
"Usage: ./" << argv[0] <<
" <robot-hostname>" 122 <<
" <a_max>" << std::endl;
126 size_t filter_size = std::stoul(argv[2]);
127 std::array<double, 7> K_P;
128 std::array<double, 7> K_D;
129 for (
size_t i = 0; i < 7; i++) {
130 K_P[i] = std::stod(argv[3]);
131 K_D[i] = std::stod(argv[4]);
134 std::cout <<
"Initializing controller: " << std::endl;
135 for (
size_t i = 0; i < 7; i++) {
136 std::cout << i + 1 <<
": K_P = " << K_P[i] <<
"\tK_D = " << K_D[i] << std::endl;
138 std::cout <<
"dq filter size: " << filter_size << std::endl;
139 Controller controller(filter_size, K_P, K_D);
142 franka::Robot robot(argv[1]);
146 robot.setCollisionBehavior(
147 {{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}},
148 {{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}},
149 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
150 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
153 int joint_number = std::stoi(argv[5]);
154 std::vector<double> trajectory = generateTrajectory(std::stod(argv[6]));
156 robot.control([&](
const franka::RobotState &robot_state,
157 franka::Duration) -> franka::Torques {
return controller.step(robot_state); },
158 [&](
const franka::RobotState &, franka::Duration time_step) -> franka::JointVelocities {
159 index += time_step.toMSec();
161 if (index >= trajectory.size()) {
162 index = trajectory.size() - 1;
165 franka::JointVelocities velocities{{0, 0, 0, 0, 0, 0, 0}};
166 velocities.dq[joint_number] = trajectory[index];
168 if (index >= trajectory.size() - 1) {
169 return franka::MotionFinished(velocities);
173 }
catch (
const franka::Exception &e) {
174 std::cout << e.what() << std::endl;
182 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }