Skip to content

Disable rate limiter #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
marcbone wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Disable rate limiter #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ae7e653
remove commented out code
Jul 27, 2023
44afb68
use switch case for derivatives
Jul 27, 2023
20335eb
disable rate limiter
Jul 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/Payload.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@
* The inertia matrix of the payloads is calculated by its type
*/
#include <liborl/Payload.h>

using namespace orl;

Payload Payloads::Sphere(double radius, double mass, const std::array<double, 3> &position_wrt_flange) {
double i = (2.0 / 5.0) * mass * radius * radius;
std::array<double, 9> inertia_matrix = {i, 0, 0, 0, i, 0, 0, 0, i};
Expand Down
6 changes: 2 additions & 4 deletions src/Pose.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
// Copyright (c) 2020 Marco Boneberger
// Licensed under the EUPL-1.2-or-later
#include <liborl/Pose.h>

using namespace orl;
using namespace Eigen;



void Orientation::set_RPY(double r, double p, double y) {
Eigen::Matrix3d a;
a = AngleAxisd(y, Vector3d::UnitZ())
Expand Down Expand Up @@ -54,8 +54,6 @@ Orientation Orientation::inverse() const {
}

Pose::Pose() {
// pose
// orientation.set_RPY(0, 0, 0);
}

Pose::Pose(std::array<double, 16> mat) {
Expand Down Expand Up @@ -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());
}
}
10 changes: 5 additions & 5 deletions src/PoseGenerator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

}
Expand All @@ -222,7 +222,7 @@ PoseGenerator PoseGenerators::BezierMotion(const std::vector<Position> &points,
std::vector<Position> 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);
Expand Down Expand Up @@ -270,7 +270,7 @@ PoseGenerator PoseGenerators::BSplineMotion(const std::vector<Position> &points,
std::vector<Position> 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);
Expand Down
4 changes: 2 additions & 2 deletions src/QuinticPolynomial.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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));
}
}

Expand Down
108 changes: 17 additions & 91 deletions src/Robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -57,7 +51,6 @@ std::array<double, 7> 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!
Expand Down Expand Up @@ -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}},
Expand Down Expand Up @@ -181,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;
Expand All @@ -197,10 +187,7 @@ void Robot::move_cartesian(PoseGenerator cartesian_pose_generator, double max_ti
const boost::optional<StopCondition> &stop_condition, boost::optional<double> elbow) {
Pose initial_pose(robot.readOnce().O_T_EE_c);
std::array<double, 2> initial_elbow;

double time = 0.0;

// std::cout << "Start motion..." << std::endl;
bool should_stop = false;
try {
robot.control(
Expand All @@ -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<double, 16> 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});
}
Expand All @@ -265,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;
Expand Down Expand Up @@ -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<<std::endl;
// Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
// but should not be done in a control loop.
Eigen::Vector3d force;
force << robot_state.O_F_ext_hat_K[0], robot_state.O_F_ext_hat_K[1], robot_state.O_F_ext_hat_K[2];
// std::cout << (start_force - force).norm() << std::endl;
// std::cout << touch_counter << std::endl;
if ((start_force - force).norm() > 2 and can_be_touched_again) {
touch_counter++;
can_be_touched_again = false;
Expand Down Expand Up @@ -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<<std::endl;
// Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
// but should not be done in a control loop.
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];
// std::cout << (start_force - force).norm() << std::endl;
// std::cout << touch_counter << std::endl;

return std::abs(start_force[axis] - force_v[axis]) < force;
});
}
Expand All @@ -390,22 +334,15 @@ bool Robot::force_to_continue(double force, Axis axis, double time_out) {
Eigen::Vector3d start_force;
start_force << s.O_F_ext_hat_K[0], s.O_F_ext_hat_K[1], s.O_F_ext_hat_K[2];
int counter = 0;
robot.read(
[&](
const franka::RobotState &robot_state) {
counter++;
// std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl<<std::endl;
// Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
// but should not be done in a control loop.
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];
// std::cout << (start_force - force).norm() << std::endl;
// std::cout << touch_counter << std::endl;
if (counter > 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;
}

Expand All @@ -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<<std::endl;
// Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
// but should not be done in a control loop.
Eigen::Vector3d force_v;
force_v << robot_state.O_F_ext_hat_K[3], robot_state.O_F_ext_hat_K[4], robot_state.O_F_ext_hat_K[5];
// std::cout << (start_force - force).norm() << std::endl;
// std::cout << touch_counter << std::endl;

return std::abs(start_force[axis] - force_v[axis]) < torque;
});
}
Expand All @@ -441,13 +372,8 @@ bool Robot::torque_to_continue(double torque, Axis axis, double time_out) {
[&](
const franka::RobotState &robot_state) {
counter++;
// std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl<<std::endl;
// Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
// but should not be done in a control loop.
Eigen::Vector3d force_v;
force_v << robot_state.O_F_ext_hat_K[3], robot_state.O_F_ext_hat_K[4], robot_state.O_F_ext_hat_K[5];
// std::cout << (start_force - force).norm() << std::endl;
// std::cout << touch_counter << std::endl;
if (counter > time_out * 1000) {
return false;
}
Expand All @@ -470,5 +396,5 @@ void Robot::absolute_cart_motion(double x, double y, double z, double max_time,

void Robot::joint_motion(std::array<double, 7> q_goal, double speed_factor) {
MotionGenerator motion_generator(speed_factor, q_goal);
robot.control(motion_generator);
}
robot.control(motion_generator, franka::ControllerMode::kJointImpedance, false);
}
Loading