From ae7e653d1e2a0251ce4330ebd973aa3f6f7898d2 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 27 Jul 2023 08:39:39 +0200 Subject: [PATCH 1/3] remove commented out code --- src/Payload.cpp | 2 + src/Pose.cpp | 6 +-- src/PoseGenerator.cpp | 10 ++-- src/QuinticPolynomial.hpp | 4 +- src/Robot.cpp | 100 +++++--------------------------------- src/SpeedProfile.cpp | 1 + src/StopCondition.cpp | 1 - src/geometry_math.cpp | 12 ++--- test/test.cpp | 67 +------------------------ 9 files changed, 30 insertions(+), 173 deletions(-) diff --git a/src/Payload.cpp b/src/Payload.cpp index 71d9f9a..1955212 100644 --- a/src/Payload.cpp +++ b/src/Payload.cpp @@ -5,7 +5,9 @@ * The inertia matrix of the payloads is calculated by its type */ #include + using namespace orl; + Payload Payloads::Sphere(double radius, double mass, const std::array &position_wrt_flange) { double i = (2.0 / 5.0) * mass * radius * radius; std::array inertia_matrix = {i, 0, 0, 0, i, 0, 0, 0, i}; diff --git a/src/Pose.cpp b/src/Pose.cpp index 513a867..55befee 100644 --- a/src/Pose.cpp +++ b/src/Pose.cpp @@ -1,11 +1,11 @@ // Copyright (c) 2020 Marco Boneberger // Licensed under the EUPL-1.2-or-later #include + using namespace orl; using namespace Eigen; - void Orientation::set_RPY(double r, double p, double y) { Eigen::Matrix3d a; a = AngleAxisd(y, Vector3d::UnitZ()) @@ -54,8 +54,6 @@ Orientation Orientation::inverse() const { } Pose::Pose() { -// pose -// orientation.set_RPY(0, 0, 0); } Pose::Pose(std::array mat) { @@ -179,4 +177,4 @@ Eigen::Affine3d::LinearMatrixType Pose::rotation() const { void Pose::add_RPY(double r, double p, double y) { setOrientation(Orientation(r, p, y) * quaternion()); -} \ No newline at end of file +} diff --git a/src/PoseGenerator.cpp b/src/PoseGenerator.cpp index 84303e9..10c1490 100644 --- a/src/PoseGenerator.cpp +++ b/src/PoseGenerator.cpp @@ -188,15 +188,15 @@ PoseGenerator PoseGenerators::AbsoluteMotion(const Position &translation, Frame case Frame::RobotTCP: throw std::invalid_argument("RobotTCP in absolute motion does not make any sense"); case Frame::RobotBase: - goal = start = input.initial_pose.getPosition(); + start = input.initial_pose.getPosition(); goal = translation; break; case Frame::UnitFrame: std::cerr << "Absolute motion in Unit Frame equals Relative Motion in Unit Frame. Use Relative Motion the next time" << std::endl; - goal = start = {0, 0, 0}; - goal = (translation); + start = {0, 0, 0}; + goal = translation; break; } @@ -222,7 +222,7 @@ PoseGenerator PoseGenerators::BezierMotion(const std::vector &points, std::vector points_with_start{start.getPosition()}; orl::Pose goal = start; goal.setPosition(points.at(points.size() - 1)); - for (const auto &point : points) { + for (const auto &point: points) { points_with_start.push_back(point); } auto position_generator = generate_bezier_curve(points_with_start); @@ -270,7 +270,7 @@ PoseGenerator PoseGenerators::BSplineMotion(const std::vector &points, std::vector points_with_start{start.getPosition()}; orl::Pose goal = start; goal.setPosition(points.at(points.size() - 1)); - for (const auto &point : points) { + for (const auto &point: points) { points_with_start.push_back(point); } auto position_generator = generate_b_spline(points_with_start, degree, knot_vector); diff --git a/src/QuinticPolynomial.hpp b/src/QuinticPolynomial.hpp index dde3a06..46a4862 100644 --- a/src/QuinticPolynomial.hpp +++ b/src/QuinticPolynomial.hpp @@ -50,7 +50,7 @@ class QuinticPolynomial { double tau = (time - start_time) / (deltaT); for (int i = 0; i < dof; ++i) { ret(i) = q_i(i) + - delta_q(i) * (6.0 * std::pow(tau, 5.0) - 15.0 * std::pow(tau, 4.0) + 10.0 * std::pow(tau, 3.0)); + delta_q(i) * (6.0 * std::pow(tau, 5.0) - 15.0 * std::pow(tau, 4.0) + 10.0 * std::pow(tau, 3.0)); } return ret; } @@ -63,7 +63,7 @@ class QuinticPolynomial { double tau = (time - start_time) / (deltaT); for (int i = 0; i < dof; ++i) { _ret(i) = q_i(i) + - delta_q(i) * (6.0 * std::pow(tau, 5.0) - 15.0 * std::pow(tau, 4.0) + 10.0 * std::pow(tau, 3.0)); + delta_q(i) * (6.0 * std::pow(tau, 5.0) - 15.0 * std::pow(tau, 4.0) + 10.0 * std::pow(tau, 3.0)); } } diff --git a/src/Robot.cpp b/src/Robot.cpp index e900e22..b1c4d95 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -30,12 +30,6 @@ void Robot::cart_motion(const Pose &dest, const double max_time, const boost::op void Robot::close_gripper(double width, double speed, double force, double epsilon_width) { - /* - * Maximum tolerated deviation when the actual grasped width is smaller than the commanded grasp width. -[in] -epsilon_outer -Maximum tolerated deviation when the actual grasped width is larger than the commanded grasp width. - */ gripper.grasp(width, speed, force, epsilon_width, epsilon_width); } @@ -57,7 +51,6 @@ std::array Robot::get_current_Joints() { Robot::Robot(const std::string &robot_name) : robot(robot_name), gripper(robot_name), model(robot.loadModel()) { robot.setGuidingMode({true, true, true, true, true, true}, false); -// robot.automaticErrorRecovery(); // Set additional parameters always before the control loop, NEVER in the control loop! @@ -105,17 +98,14 @@ Robot::impedance_mode(double translational_stiffness, double rotational_stiffnes stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); damping.setZero(); damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * - Eigen::MatrixXd::Identity(3, 3); + Eigen::MatrixXd::Identity(3, 3); damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * - Eigen::MatrixXd::Identity(3, 3); + Eigen::MatrixXd::Identity(3, 3); try { // connect to robot franka::RobotState initial_state = robot.readOnce(); orl::Pose initial_pose(initial_state.O_T_EE); - // equilibrium point is the initial position -// Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(desired_attractor_pose.to_matrix().data())); -// Eigen::Vector3d position_d(initial_transform.translation()); -// Eigen::Quaterniond orientation_d(initial_transform.linear()); + // set collision behavior robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, @@ -197,10 +187,7 @@ void Robot::move_cartesian(PoseGenerator cartesian_pose_generator, double max_ti const boost::optional &stop_condition, boost::optional elbow) { Pose initial_pose(robot.readOnce().O_T_EE_c); std::array initial_elbow; - double time = 0.0; - -// std::cout << "Start motion..." << std::endl; bool should_stop = false; try { robot.control( @@ -216,46 +203,14 @@ void Robot::move_cartesian(PoseGenerator cartesian_pose_generator, double max_ti const double progress = time / max_time; PoseGeneratorInput generatorInput{progress, initial_pose, state}; Pose new_pose_pose = cartesian_pose_generator(generatorInput); - if (time == 0) { -// std::cout << "desired pose: " << new_pose_pose.toString() << std::endl; -// std::cout << "start: " << initial_pose.toString() << std::endl; - } std::array new_pose = new_pose_pose.to_matrix(); -// std::cout << new_pose_pose.toString() << std::endl << std::endl; -// std::cout << Pose(state.O_T_EE).toString() << std::endl << std::endl; -// for (int i = 0; i < 4; i++) { -// for (int j = 0; j < 4; j++) { -// std::cout << new_pose[i + 4 * j] << "\t"; -// } -// std::cout << std::endl; -// } -// Eigen::Vector3d a; -// a << new_pose[0], new_pose[1], new_pose[2]; -// Eigen::Vector3d b; -// b << new_pose[4], new_pose[5], new_pose[6]; -// Eigen::Vector3d c; -// c << new_pose[8], new_pose[9], new_pose[10]; -// std::cout << a.norm() << " " << b.norm() << " " << c.norm() << std::endl; -// std::cout << std::endl; - - // Print the xyz-forces at the EE in every step of control loop: -// double force_norm = sqrt(state.O_F_ext_hat_K[0] * state.O_F_ext_hat_K[0] + -// state.O_F_ext_hat_K[1] * state.O_F_ext_hat_K[1] + -// state.O_F_ext_hat_K[2] * state.O_F_ext_hat_K[2]); -// std::cout << "Force: " << force_norm << std::endl; if (elbow.is_initialized()) { new_elbow[0] += (elbow.value() - initial_elbow[0]) * (1 - std::cos(M_PI * progress)) / 2.0; } if (stop_condition.is_initialized()) { should_stop = stop_condition.value()(generatorInput); - - } if (time >= max_time or should_stop) { - if (should_stop) { -// std::cout << "Stopped motion by stop condition" << std::endl; - } -// std::cout << std::endl << "Finished motion, shutting down example" << std::endl; if (elbow.is_initialized()) { return franka::MotionFinished({new_pose, new_elbow}); } @@ -329,13 +284,8 @@ void Robot::double_tap_robot_to_continue() { robot.read( [&]( const franka::RobotState &robot_state) { -// std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl< 2 and can_be_touched_again) { touch_counter++; can_be_touched_again = false; @@ -372,14 +322,8 @@ void Robot::force_to_continue(double force, Axis axis) { robot.read( [&]( const franka::RobotState &robot_state) { -// std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl< time_out * 1000) { - return false; - } - return std::abs(start_force[axis] - force_v[axis]) < force; - }); + robot.read([&](const franka::RobotState &robot_state) { + counter++; + Eigen::Vector3d force_v; + force_v << robot_state.O_F_ext_hat_K[0], robot_state.O_F_ext_hat_K[1], robot_state.O_F_ext_hat_K[2]; + if (counter > time_out * 1000) { + return false; + } + return std::abs(start_force[axis] - force_v[axis]) < force; + }); return counter < time_out * 1000; } @@ -418,14 +355,8 @@ void Robot::torque_to_continue(double torque, Axis axis) { robot.read( [&]( const franka::RobotState &robot_state) { -// std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl< time_out * 1000) { return false; } @@ -471,4 +397,4 @@ void Robot::absolute_cart_motion(double x, double y, double z, double max_time, void Robot::joint_motion(std::array q_goal, double speed_factor) { MotionGenerator motion_generator(speed_factor, q_goal); robot.control(motion_generator); -} \ No newline at end of file +} diff --git a/src/SpeedProfile.cpp b/src/SpeedProfile.cpp index 1d6162b..f8a2d9b 100644 --- a/src/SpeedProfile.cpp +++ b/src/SpeedProfile.cpp @@ -3,6 +3,7 @@ #include #include "QuinticPolynomial.hpp" #include + using namespace orl; void orl::apply_speed_profile(PoseGenerator &pose_generator, const SpeedProfile &speed_profile) { diff --git a/src/StopCondition.cpp b/src/StopCondition.cpp index b2a886a..eb4fbf0 100644 --- a/src/StopCondition.cpp +++ b/src/StopCondition.cpp @@ -7,7 +7,6 @@ StopCondition StopConditions::Force(double max_force, double minimum_progress) { StopCondition stop_condition = [=](const PoseGeneratorInput &input) { Eigen::Vector3d force; force << input.state.O_F_ext_hat_K[0], input.state.O_F_ext_hat_K[1], input.state.O_F_ext_hat_K[2]; -// std::cout << "force: " << force.norm() << std::endl; return force.norm() > max_force and input.progress > minimum_progress; }; return stop_condition; diff --git a/src/geometry_math.cpp b/src/geometry_math.cpp index 68881b0..c5c4b5f 100644 --- a/src/geometry_math.cpp +++ b/src/geometry_math.cpp @@ -20,19 +20,13 @@ generate_circle_movement(const Eigen::Vector3d ¢er, const Eigen::Vector3d &n //project center: const double radius = std::abs((center - start).norm()); -// std::cout << "normal: " << normal << std::endl; Eigen::Vector3d center_direction = center - start; // v1 = [0,1,0] x normal aka the new y direction auto v1 = normal.cross(center_direction); -// v1 << normal[2], 0, -normal[0]; v1.normalize(); -// std::cout << "v1: " << v1 << std::endl; -// std::cout << "radius: " << radius << std::endl; Eigen::Vector3d v2 = normal.cross(v1); // the new x direction -// std::cout << "v2: " << v2 << std::endl; auto tmp_start = radius * v1; double start_angle = angle_between(start - center, tmp_start, normal); -// std::cout << "start angle: " << start_angle << std::endl; std::function circle_generator = [=](double progress) -> Eigen::Vector3d { double desired_angle = start_angle + progress * rotation_angle; Eigen::Vector3d point_on_circle = @@ -139,8 +133,8 @@ generate_b_spline(const std::vector &points, int degree, const if (knot_vector.size() != points.size() + degree + 1) { std::stringstream ss; ss << "The knot vector has the wrong length. It should have #points + degree of polynomial + 1 = " - << points.size() + degree + 1 << " components." << std::endl << "But it has " << knot_vector.size() - << " components"; + << points.size() + degree + 1 << " components." << std::endl << "But it has " << knot_vector.size() + << " components"; throw std::invalid_argument(ss.str()); } auto b_spline_generator = [=](double progress) { @@ -156,4 +150,4 @@ generate_b_spline(const std::vector &points, int degree, const return b_spline_de_boor(k, p, progress, knot_vector, points); }; return b_spline_generator; -} \ No newline at end of file +} diff --git a/test/test.cpp b/test/test.cpp index d859db6..933b70a 100644 --- a/test/test.cpp +++ b/test/test.cpp @@ -8,11 +8,7 @@ using namespace orl; TEST(PoseGeneratorTest, LinearInterpolation) { orl::Pose start_pose; -// start_pose.orientation.set_RPY(0, 0, 0); -// start_pose.position.set(0, 0, 0); orl::Pose end_pose; -// end_pose.orientation.set_RPY(0, 0, 0); -// end_pose.position.set(1, 1, 1); auto pose_generator = orl::generate_pose_interpolation_pose_generator(end_pose); orl::apply_speed_profile(pose_generator, orl::SpeedProfiles::LinearProfile()); auto pose_generator_quintic = orl::generate_pose_interpolation_pose_generator(end_pose); @@ -21,8 +17,6 @@ TEST(PoseGeneratorTest, LinearInterpolation) { for (int i = 0; i < 100; i++) { double progress = i / 100.; orl::PoseGeneratorInput input{progress, start_pose, state}; -// std::cout << "1: " << pose_generator(input).toString() << std::endl; -// std::cout << "2: " << pose_generator_quintic(input).toString() << std::endl; } } @@ -44,9 +38,6 @@ TEST(PoseGeneratorTest, ConstantPositionTest) { ASSERT_NEAR(generator_quat.x(), quat.x(), 0.00001); ASSERT_NEAR(generator_quat.y(), quat.y(), 0.00001); ASSERT_NEAR(generator_quat.z(), quat.z(), 0.00001); -// std::cout< points{{0, 0, 0}, {0, 1, 0}, -// {2, 1, 0}, -// {2, 0, 0} }; auto pose_generator = generate_b_spline(points, 1); @@ -334,11 +283,7 @@ TEST(GeometryTest, BSplineDegree1Test) { Position generator_position(pose_generator(progress)); Position desired_position; - desired_position << 0, progress, 0; -// std::cout << "desired " << desired_position.x() << "\t" << desired_position.y() << "\t" << desired_position.z() -// << "\t" << std::endl; -// std::cout << "generat " << generator_position.x() << "\t" << generator_position.y() << "\t" -// << generator_position.z() << "\t" << std::endl; + desired_position << 0, progress, 0;; ASSERT_NEAR((generator_position - desired_position).norm(), 0, 0.0001); } } @@ -385,10 +330,6 @@ TEST(GeometryTest, BSplineMotionTest) { Position generator_position(pose_generator(input).getPosition()); Position desired_position; desired_position << desired_x.at(i), desired_y.at(i), 0; -// std::cout << "desired " << desired_position.x() << "\t" << desired_position.y() << "\t" << desired_position.z() -// << "\t" << std::endl; -// std::cout << "generat " << generator_position.x() << "\t" << generator_position.y() << "\t" -// << generator_position.z() << "\t" << std::endl; ASSERT_NEAR((generator_position - desired_position).norm(), 0, 0.00001); } } @@ -566,10 +507,6 @@ TEST(PoseGeneratorTest, SCurve) { Position generator_position(pose_generator(input).getPosition()); Position desired_position; desired_position << 1, 0, 1 + desired_positions[i]; -// std::cout << "desired " << desired_position.x() << "\t" << desired_position.y() << "\t" << desired_position.z() -// << "\t" << std::endl; -// std::cout << "generat " << generator_position.x() << "\t" << generator_position.y() << "\t" -// << generator_position.z() << "\t" << std::endl; ASSERT_NEAR((generator_position - desired_position).norm(), 0, 0.00001); } } @@ -577,4 +514,4 @@ TEST(PoseGeneratorTest, SCurve) { int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 44afb68eafbaa300d3cb4842fc502f2a384b59c6 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 27 Jul 2023 08:50:04 +0200 Subject: [PATCH 2/3] use switch case for derivatives --- src/SCurve.cpp | 218 ++++++++++++++++++++++++++----------------------- 1 file changed, 114 insertions(+), 104 deletions(-) diff --git a/src/SCurve.cpp b/src/SCurve.cpp index 85060cc..b556217 100644 --- a/src/SCurve.cpp +++ b/src/SCurve.cpp @@ -9,63 +9,64 @@ double SCurve::deceleration_phase(const SCurveTrajectoryParameters ¶meters, const auto × = parameters.times; if (t <= times.T() - times.T_d + times.T_j2) { - - if (derivative == Derivative::Position) { - return parameters.s.q1 - (parameters.v_lim + parameters.s.v1) * times.T_d / 2 + - parameters.v_lim * (t - times.T() + times.T_d) - parameters.j_max * pow( - t - times.T() + times.T_d, 3) / 6; - } - if (derivative == Derivative::Velocity) { - return parameters.v_lim - parameters.j_max * pow(t - times.T() + times.T_d, 2) / 2; - } - if (derivative == Derivative::Acceleration) { - return -parameters.j_max * (t - times.T() + times.T_d); + switch (derivative) { + case Position: + return parameters.s.q1 - (parameters.v_lim + parameters.s.v1) * times.T_d / 2 + + parameters.v_lim * (t - times.T() + times.T_d) - parameters.j_max * pow( + t - times.T() + times.T_d, 3) / 6; + break; + case Velocity: + return parameters.v_lim - parameters.j_max * pow(t - times.T() + times.T_d, 2) / 2; + break; + case Acceleration: + return -parameters.j_max * (t - times.T() + times.T_d); + break; + case Jerk: + return parameters.j_min; + break; } - if (derivative == Derivative::Jerk) { - return parameters.j_min; + } else if (t <= times.T() - times.T_j2) { + switch (derivative) { + case Position: + return parameters.s.q1 - (parameters.v_lim + parameters.s.v1) * times.T_d / 2 + + parameters.v_lim * (t - times.T() + times.T_d) + + parameters.a_lim_d / 6 * ( + 3 * pow(t - times.T() + times.T_d, 2) - 3 * times.T_j2 * ( + t - times.T() + times.T_d) + pow(times.T_j2, 2)); + break; + case Velocity: + return parameters.v_lim + parameters.a_lim_d * (t - times.T() + times.T_d - times.T_j2 / 2); + break; + case Acceleration: + return parameters.a_lim_d; + break; + case Jerk: + return 0; + break; } - } - if (t <= times.T() - times.T_j2) { - - if (derivative == Derivative::Position) { - return parameters.s.q1 - (parameters.v_lim + parameters.s.v1) * times.T_d / 2 + - parameters.v_lim * (t - times.T() + times.T_d) + - parameters.a_lim_d / 6 * ( - 3 * pow(t - times.T() + times.T_d, 2) - 3 * times.T_j2 * ( - t - times.T() + times.T_d) + pow(times.T_j2, 2)); + } else if (t <= times.T()) { + switch (derivative) { + case Position: + return parameters.s.q1 - parameters.s.v1 * (times.T() - t) - + parameters.j_max * pow(times.T() - t, 3) / 6; + break; + case Velocity: + return parameters.s.v1 + parameters.j_max * pow(times.T() - t, 2) / 2; + break; + case Acceleration: + return -parameters.j_max * (times.T() - t); + break; + case Jerk: + return parameters.j_max; + break; } - if (derivative == Derivative::Velocity) { - return parameters.v_lim + parameters.a_lim_d * (t - times.T() + times.T_d - times.T_j2 / 2); - } - if (derivative == Derivative::Acceleration) { - return parameters.a_lim_d; - } - if (derivative == Derivative::Jerk) { - return 0; - } - } - if (t <= times.T()) { + std::stringstream ss; + ss << "t should be between " << times.T_a + times.T_v << " and " << times.T() << " but it is " << t; - if (derivative == Derivative::Position) { - return parameters.s.q1 - parameters.s.v1 * (times.T() - t) - parameters.j_max * pow(times.T() - t, 3) / 6; - } - if (derivative == Derivative::Velocity) { - return parameters.s.v1 + parameters.j_max * pow(times.T() - t, 2) / 2; - } - if (derivative == Derivative::Acceleration) { - return -parameters.j_max * (times.T() - t); - } - if (derivative == Derivative::Jerk) { - return parameters.j_max; - } - } else { - std::stringstream ss; - ss << "t should be between " << times.T_a + times.T_v << " and " << times.T() << " but it is " << t; + throw std::invalid_argument(ss.str()); - throw std::invalid_argument(ss.str()); - } } double SCurve::eval_s_curve(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { @@ -88,74 +89,83 @@ double SCurve::eval_s_curve(const SCurveTrajectoryParameters ¶meters, double double SCurve::constant_velocity_phase(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { const auto × = parameters.times; - if (derivative == Derivative::Position) { - return parameters.s.q0 + (parameters.v_lim + parameters.s.v0) * times.T_a / 2 + - parameters.v_lim * (t - times.T_a); - } - if (derivative == Derivative::Velocity) { - return parameters.v_lim; - } - if (derivative == Derivative::Acceleration) { - return 0; - } - if (derivative == Derivative::Jerk) { - return 0; + switch (derivative) { + case Position: + return parameters.s.q0 + (parameters.v_lim + parameters.s.v0) * times.T_a / 2 + + parameters.v_lim * (t - times.T_a); + break; + case Velocity: + return parameters.v_lim; + break; + case Acceleration: + return 0; + break; + case Jerk: + return 0; + break; } + std::stringstream ss; + ss << "Invalid derivative given: " << derivative; + throw std::invalid_argument(ss.str()); } double SCurve::acceleration_phase(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { const auto × = parameters.times; if (t <= times.T_j1) { - if (derivative == Derivative::Position) { - return parameters.s.q0 + parameters.s.v0 * t + parameters.j_max * pow(t, 3) / 6; - } - if (derivative == Derivative::Velocity) { - return parameters.s.v0 + parameters.j_max * pow(t, 2) / 2; - } - if (derivative == Derivative::Acceleration) { - return parameters.j_max * t; - } - if (derivative == Derivative::Jerk) { - return parameters.j_max; + switch (derivative) { + case Position: + return parameters.s.q0 + parameters.s.v0 * t + parameters.j_max * pow(t, 3) / 6; + break; + case Velocity: + return parameters.s.v0 + parameters.j_max * pow(t, 2) / 2; + break; + case Acceleration: + return parameters.j_max * t; + break; + case Jerk: + return parameters.j_max; + break; } } if (t <= times.T_a - times.T_j1) { - - if (derivative == Derivative::Position) { - return parameters.s.q0 + parameters.s.v0 * t + - parameters.a_lim_a / 6 * (3 * pow(t, 2) - 3 * times.T_j1 * t + pow(times.T_j1, 2)); - } - if (derivative == Derivative::Velocity) { - return parameters.s.v0 + parameters.a_lim_a * (t - times.T_j1 / 2); - } - if (derivative == Derivative::Acceleration) { - return parameters.a_lim_a; - } - if (derivative == Derivative::Jerk) { - return 0; + switch (derivative) { + case Position: + return parameters.s.q0 + parameters.s.v0 * t + + parameters.a_lim_a / 6 * (3 * pow(t, 2) - 3 * times.T_j1 * t + pow(times.T_j1, 2)); + break; + case Velocity: + return parameters.s.v0 + parameters.a_lim_a * (t - times.T_j1 / 2); + break; + case Acceleration: + return parameters.a_lim_a; + break; + case Jerk: + return 0; + break; } } if (t <= times.T_a) { - - if (derivative == Derivative::Position) { - return parameters.s.q0 + (parameters.v_lim + parameters.s.v0) * times.T_a / 2 - - parameters.v_lim * (times.T_a - t) - parameters.j_min * pow( - times.T_a - t, 3) / 6; - } - if (derivative == Derivative::Velocity) { - return parameters.v_lim + parameters.j_min * pow(times.T_a - t, 2) / 2; - } - if (derivative == Derivative::Acceleration) { - return -parameters.j_min * (times.T_a - t); - } - if (derivative == Derivative::Jerk) { - return parameters.j_min; + switch (derivative) { + case Position: + return parameters.s.q0 + (parameters.v_lim + parameters.s.v0) * times.T_a / 2 - + parameters.v_lim * (times.T_a - t) - parameters.j_min * pow( + times.T_a - t, 3) / 6; + break; + case Velocity: + return parameters.v_lim + parameters.j_min * pow(times.T_a - t, 2) / 2; + break; + case Acceleration: + return -parameters.j_min * (times.T_a - t); + break; + case Jerk: + return parameters.j_min; + break; } - } else { - std::stringstream ss; - ss << "t should be between " << 0 << " and " << times.T_a << " but it is " << t; - throw std::invalid_argument(ss.str()); } + std::stringstream ss; + ss << "t should be between " << 0 << " and " << times.T_a << " but it is " << t; + throw std::invalid_argument(ss.str()); + } SCurveTrajectoryParameters SCurve::generate_trajectory_parameters(const SCurveTimings ×, From 20335ebedc42a283ff4a4a4f20744d5b434721ad Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 27 Jul 2023 08:52:49 +0200 Subject: [PATCH 3/3] disable rate limiter --- src/Robot.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index b1c4d95..9abab8e 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -171,7 +171,7 @@ Robot::impedance_mode(double translational_stiffness, double rotational_stiffnes } return tau_d_array; }; - robot.control(impedance_control_callback); + robot.control(impedance_control_callback, false); } catch (const franka::Exception &ex) { // print exception std::cout << ex.what() << std::endl; @@ -220,8 +220,8 @@ void Robot::move_cartesian(PoseGenerator cartesian_pose_generator, double max_ti return {new_pose, new_elbow}; } return new_pose; - }, franka::ControllerMode::kCartesianImpedance); - } catch (franka::Exception exception) { + }, franka::ControllerMode::kCartesianImpedance, false); + } catch (const franka::Exception& exception) { std::cout << exception.what() << std::endl; if (should_stop) { std::cout << "robot has stopped due to StopCondition" << std::endl; @@ -396,5 +396,5 @@ void Robot::absolute_cart_motion(double x, double y, double z, double max_time, void Robot::joint_motion(std::array q_goal, double speed_factor) { MotionGenerator motion_generator(speed_factor, q_goal); - robot.control(motion_generator); + robot.control(motion_generator, franka::ControllerMode::kJointImpedance, false); }