From 8b67c48047ae1e953b533c2a93a96077df54417d Mon Sep 17 00:00:00 2001 From: Philipp Date: Fri, 8 Mar 2019 15:52:38 -0800 Subject: [PATCH 1/6] kinematics --- .../src/blue_kinematics.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/blue_hardware_interface/src/blue_kinematics.cpp b/blue_hardware_interface/src/blue_kinematics.cpp index caa451c3..90c756be 100644 --- a/blue_hardware_interface/src/blue_kinematics.cpp +++ b/blue_hardware_interface/src/blue_kinematics.cpp @@ -395,12 +395,21 @@ std::vector BlueKinematics::getActuatorCommands( // Soft stops // TODO: hacky and temporary - if(joint_pos_[i] > softstop_max_angles[i] - softstop_tolerance) { - double offset = joint_pos_[i] - softstop_max_angles[i] + softstop_tolerance; - joint_cmd_[i] += -1.0 * softstop_torque_limit * pow(offset, 2); - } else if (joint_pos_[i] < softstop_min_angles[i] + softstop_tolerance) { - double offset = softstop_min_angles[i] + softstop_tolerance - joint_pos_[i]; - joint_cmd_[i] += softstop_torque_limit * pow(offset, 2); + if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance) ) { + double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); + joint_cmd_[i] += -1.0 * softstop_torque_limit * abs(offset, 2); + + // apply d term to softstop + if (joint_vel_[i] > 0) + joint_cmd_[i] += -10.0 * softstop_torque_limit * abs(joint_vel_[i]); + + } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance) ) { + double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; + joint_cmd_[i] += softstop_torque_limit * abs(offset, 2); + + // apply d term to softstop + if (joint_vel_[i] < 0) + joint_cmd_[i] += 10.0 * softstop_torque_limit * abs(joint_vel_[i]); } } From 2f02b9e0a72a1702e4c971315c46bcb1c2b4287c Mon Sep 17 00:00:00 2001 From: Philipp Date: Fri, 8 Mar 2019 16:15:31 -0800 Subject: [PATCH 2/6] kin --- blue_hardware_interface/src/blue_kinematics.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/blue_hardware_interface/src/blue_kinematics.cpp b/blue_hardware_interface/src/blue_kinematics.cpp index 90c756be..0d06d51c 100644 --- a/blue_hardware_interface/src/blue_kinematics.cpp +++ b/blue_hardware_interface/src/blue_kinematics.cpp @@ -385,18 +385,15 @@ std::vector BlueKinematics::getActuatorCommands( // Compute joint commands for (int i = 0; i < num_joints_; i++) { - // Add feedforward if it exists - if (i < feedforward_torques.size()) - joint_cmd_[i] = raw_joint_cmd_[i] + feedforward_torques[i]; - else - joint_cmd_[i] = raw_joint_cmd_[i]; - + joint_cmd_[i] = raw_joint_cmd_[i]; raw_joint_cmd_[i] = 0.0; // Soft stops // TODO: hacky and temporary if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance) ) { double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); + double rational = 1.0 / pow(offset, 2); + joint_cmd_[i] = max(joint_cmd_[i], -rational) joint_cmd_[i] += -1.0 * softstop_torque_limit * abs(offset, 2); // apply d term to softstop @@ -405,12 +402,18 @@ std::vector BlueKinematics::getActuatorCommands( } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance) ) { double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; + double rational = 1.0 / pow(offset, 2); + joint_cmd_[i] = min(joint_cmd_[i], rational) joint_cmd_[i] += softstop_torque_limit * abs(offset, 2); // apply d term to softstop if (joint_vel_[i] < 0) joint_cmd_[i] += 10.0 * softstop_torque_limit * abs(joint_vel_[i]); } + + // Add feedforward if it exists + if (i < feedforward_torques.size()) + joint_cmd_[i] = joint_cmd_[i] + feedforward_torques[i]; } // Propagate through transmissions to compute actuator commands From ac2f0472d8176718bda91c7d41ba43dad469839d Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Fri, 8 Mar 2019 21:13:49 -0600 Subject: [PATCH 3/6] updated softstops --- .../config/robot_parameters_left.yaml | 6 ++-- .../config/robot_parameters_right.yaml | 2 +- .../src/blue_kinematics.cpp | 36 ++++++++++--------- 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/blue_bringup/config/robot_parameters_left.yaml b/blue_bringup/config/robot_parameters_left.yaml index 7dd2ac29..7392cda1 100644 --- a/blue_bringup/config/robot_parameters_left.yaml +++ b/blue_bringup/config/robot_parameters_left.yaml @@ -21,9 +21,9 @@ blue_hardware: gear_ratios: [7.1875, 7.1875, 8.2444852941, 7.1875, 8.24448529412, 7.1875, 8.24448529412, 14.4] joint_torque_directions: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] id_torque_gains: [1.12, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] - softstop_min_angles: [-2.2, -2.25, -2.6, -2.25, -2.6, -2.25, -2.6, -1.45] - softstop_max_angles: [3.3, 0.5, 2.6, .5, 2.6, 0.5, 2.6, 1.45] - softstop_tolerance: 0.19 + softstop_min_angles: [-2.2, -2.35, -2.6, -2.35, -2.6, -2.35, -2.6, -1.45] + softstop_max_angles: [ 3.3, 0.2, 2.6, 0.2, 2.6, 0.2, 2.6, 1.45] + softstop_tolerance: 0.2 softstop_torque_limit: 95.0 ### Motor configuration diff --git a/blue_bringup/config/robot_parameters_right.yaml b/blue_bringup/config/robot_parameters_right.yaml index e1b5395c..d313d139 100644 --- a/blue_bringup/config/robot_parameters_right.yaml +++ b/blue_bringup/config/robot_parameters_right.yaml @@ -22,7 +22,7 @@ blue_hardware: joint_torque_directions: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] id_torque_gains: [1.12, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] softstop_min_angles: [-3.3, -2.25, -2.6, -2.25, -2.6, -2.25, -2.6, -1.45] - softstop_max_angles: [2.2, 0.5, 2.6, 0.5, 2.6, 0.2, 2.6, 1.45] + softstop_max_angles: [ 2.2, 0.5, 2.6, 0.5, 2.6, 0.2, 2.6, 1.45] softstop_tolerance: 0.19 softstop_torque_limit: 95.0 diff --git a/blue_hardware_interface/src/blue_kinematics.cpp b/blue_hardware_interface/src/blue_kinematics.cpp index 0d06d51c..44189ba7 100644 --- a/blue_hardware_interface/src/blue_kinematics.cpp +++ b/blue_hardware_interface/src/blue_kinematics.cpp @@ -390,25 +390,29 @@ std::vector BlueKinematics::getActuatorCommands( // Soft stops // TODO: hacky and temporary - if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance) ) { + if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance)) { double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); - double rational = 1.0 / pow(offset, 2); - joint_cmd_[i] = max(joint_cmd_[i], -rational) - joint_cmd_[i] += -1.0 * softstop_torque_limit * abs(offset, 2); - - // apply d term to softstop - if (joint_vel_[i] > 0) - joint_cmd_[i] += -10.0 * softstop_torque_limit * abs(joint_vel_[i]); + if (joint_vel_[i] > 0) { + double rational = 1.0 / pow(offset, 2); + joint_cmd_[i] = std::max(joint_cmd_[i], -rational); + joint_cmd_[i] += -0.2 * softstop_torque_limit * abs(offset); + } else { + // apply d term to softstop, scaled by the offset to keep continuity + joint_cmd_[i] += offset * (-0.3 * softstop_torque_limit * joint_vel_[i]); + joint_cmd_[i] += -0.2 * softstop_torque_limit * pow(offset, 2); + } - } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance) ) { + } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance)) { double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; - double rational = 1.0 / pow(offset, 2); - joint_cmd_[i] = min(joint_cmd_[i], rational) - joint_cmd_[i] += softstop_torque_limit * abs(offset, 2); - - // apply d term to softstop - if (joint_vel_[i] < 0) - joint_cmd_[i] += 10.0 * softstop_torque_limit * abs(joint_vel_[i]); + if (joint_vel_[i] < 0) { + double rational = 1.0 / pow(offset, 2); + joint_cmd_[i] = std::min(joint_cmd_[i], rational); + joint_cmd_[i] += 0.2 * softstop_torque_limit * abs(offset); + } else { + // apply d term to softstop + joint_cmd_[i] += offset * (-0.3 * softstop_torque_limit * joint_vel_[i]); + joint_cmd_[i] += 0.2 * softstop_torque_limit * pow(offset, 2); + } } // Add feedforward if it exists From eb7cc58504d0883509ac0598d0cca7fc6f5609aa Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Mon, 11 Mar 2019 13:52:24 -0500 Subject: [PATCH 4/6] velocity softstops --- .../config/robot_parameters_left.yaml | 6 ++-- .../blue_hardware_interface.h | 2 +- .../blue_hardware_interface/blue_kinematics.h | 2 +- .../src/blue_hardware_interface.cpp | 4 +-- .../src/blue_kinematics.cpp | 35 +++++++++---------- 5 files changed, 23 insertions(+), 26 deletions(-) diff --git a/blue_bringup/config/robot_parameters_left.yaml b/blue_bringup/config/robot_parameters_left.yaml index 7392cda1..87726030 100644 --- a/blue_bringup/config/robot_parameters_left.yaml +++ b/blue_bringup/config/robot_parameters_left.yaml @@ -21,10 +21,10 @@ blue_hardware: gear_ratios: [7.1875, 7.1875, 8.2444852941, 7.1875, 8.24448529412, 7.1875, 8.24448529412, 14.4] joint_torque_directions: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] id_torque_gains: [1.12, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] - softstop_min_angles: [-2.2, -2.35, -2.6, -2.35, -2.6, -2.35, -2.6, -1.45] - softstop_max_angles: [ 3.3, 0.2, 2.6, 0.2, 2.6, 0.2, 2.6, 1.45] + softstop_min_angles: [-2.2, -2.25, -2.6, -2.25, -2.6, -2.25, -2.6, -1.45] + softstop_max_angles: [ 2.8, 0.1, 2.6, 0.1, 2.6, 0.1, 2.6, 1.45] softstop_tolerance: 0.2 - softstop_torque_limit: 95.0 + softstop_torque_gains: [65, 10, 10, 5, 5, 1, 1, 0] ### Motor configuration motor_names: diff --git a/blue_hardware_interface/include/blue_hardware_interface/blue_hardware_interface.h b/blue_hardware_interface/include/blue_hardware_interface/blue_hardware_interface.h index 90d025b5..66a92c63 100644 --- a/blue_hardware_interface/include/blue_hardware_interface/blue_hardware_interface.h +++ b/blue_hardware_interface/include/blue_hardware_interface/blue_hardware_interface.h @@ -43,7 +43,7 @@ typedef struct { // Soft stops // TODO: hacky and temporary - double softstop_torque_limit; + std::vector softstop_torque_gains; std::vector softstop_min_angles; std::vector softstop_max_angles; double softstop_tolerance; diff --git a/blue_hardware_interface/include/blue_hardware_interface/blue_kinematics.h b/blue_hardware_interface/include/blue_hardware_interface/blue_kinematics.h index 6112aa26..ef07ffa0 100644 --- a/blue_hardware_interface/include/blue_hardware_interface/blue_kinematics.h +++ b/blue_hardware_interface/include/blue_hardware_interface/blue_kinematics.h @@ -55,7 +55,7 @@ class BlueKinematics // Get desired actuator commands std::vector getActuatorCommands( const std::vector &feedforward_torques, - double softstop_torque_limit, // TODO: clean up softstop code + const std::vector &softstop_torque_gains, const std::vector &softstop_min_angles, const std::vector &softstop_max_angles, double softstop_tolerance); diff --git a/blue_hardware_interface/src/blue_hardware_interface.cpp b/blue_hardware_interface/src/blue_hardware_interface.cpp index 90208ff8..a26f68ac 100644 --- a/blue_hardware_interface/src/blue_hardware_interface.cpp +++ b/blue_hardware_interface/src/blue_hardware_interface.cpp @@ -94,7 +94,7 @@ void BlueHW::write() { // Get actuator commands, using the gravity comp torques as a feedforward auto actuator_commands = kinematics_.getActuatorCommands( feedforward_torques, - params_.softstop_torque_limit, // TODO: clean up softstop code + params_.softstop_torque_gains, // TODO: clean up softstop code params_.softstop_min_angles, params_.softstop_max_angles, params_.softstop_tolerance); @@ -178,7 +178,7 @@ void BlueHW::loadParams() { // Soft stops // TODO: hacky and temporary - getParam("blue_hardware/softstop_torque_limit", params_.softstop_torque_limit); + getParam("blue_hardware/softstop_torque_gains", params_.softstop_torque_gains); getParam("blue_hardware/softstop_min_angles", params_.softstop_min_angles); getParam("blue_hardware/softstop_max_angles", params_.softstop_max_angles); getParam("blue_hardware/softstop_tolerance", params_.softstop_tolerance); diff --git a/blue_hardware_interface/src/blue_kinematics.cpp b/blue_hardware_interface/src/blue_kinematics.cpp index 44189ba7..ba7ac67c 100644 --- a/blue_hardware_interface/src/blue_kinematics.cpp +++ b/blue_hardware_interface/src/blue_kinematics.cpp @@ -369,7 +369,7 @@ void BlueKinematics::getGravityVectors( std::vector BlueKinematics::getActuatorCommands( const std::vector &feedforward_torques, - double softstop_torque_limit, // TODO: clean up softstop code + const std::vector &softstop_torque_gains, const std::vector &softstop_min_angles, const std::vector &softstop_max_angles, double softstop_tolerance) { @@ -390,29 +390,26 @@ std::vector BlueKinematics::getActuatorCommands( // Soft stops // TODO: hacky and temporary + double eps_vel = 0.1; if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance)) { - double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); - if (joint_vel_[i] > 0) { - double rational = 1.0 / pow(offset, 2); - joint_cmd_[i] = std::max(joint_cmd_[i], -rational); - joint_cmd_[i] += -0.2 * softstop_torque_limit * abs(offset); - } else { - // apply d term to softstop, scaled by the offset to keep continuity - joint_cmd_[i] += offset * (-0.3 * softstop_torque_limit * joint_vel_[i]); - joint_cmd_[i] += -0.2 * softstop_torque_limit * pow(offset, 2); + if(joint_pos_[i] > softstop_max_angles[i] - softstop_tolerance/2.0) { + joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; + } else if (joint_vel_[i] < 0) { + joint_cmd_[i] += -1.0 * softstop_torque_gains[i] * joint_vel_[i]; } + double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); + joint_cmd_[i] += -0.1 * softstop_torque_gains[i] * pow(offset, 1); + // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance)) { - double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; - if (joint_vel_[i] < 0) { - double rational = 1.0 / pow(offset, 2); - joint_cmd_[i] = std::min(joint_cmd_[i], rational); - joint_cmd_[i] += 0.2 * softstop_torque_limit * abs(offset); - } else { - // apply d term to softstop - joint_cmd_[i] += offset * (-0.3 * softstop_torque_limit * joint_vel_[i]); - joint_cmd_[i] += 0.2 * softstop_torque_limit * pow(offset, 2); + if(joint_pos_[i] < softstop_min_angles[i] + softstop_tolerance/2.0) { + joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; + } else if (joint_vel_[i] > 0) { + joint_cmd_[i] += -1.0 * softstop_torque_gains[i] * joint_vel_[i]; } + double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; + joint_cmd_[i] += 0.1 * softstop_torque_gains[i] * pow(offset, 1); + // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; } // Add feedforward if it exists From de9d2dd98479e35fcafd0892cc0f186747e20114 Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Thu, 21 Mar 2019 00:27:20 -0500 Subject: [PATCH 5/6] idk whats in this branch but committing :( - brent --- .../config/robot_parameters_left.yaml | 2 +- .../urdf/gripper_v1/gripper.urdf.xacro | 2 +- .../src/blue_kinematics.cpp | 24 +++++++++---------- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/blue_bringup/config/robot_parameters_left.yaml b/blue_bringup/config/robot_parameters_left.yaml index 87726030..48da6b64 100644 --- a/blue_bringup/config/robot_parameters_left.yaml +++ b/blue_bringup/config/robot_parameters_left.yaml @@ -24,7 +24,7 @@ blue_hardware: softstop_min_angles: [-2.2, -2.25, -2.6, -2.25, -2.6, -2.25, -2.6, -1.45] softstop_max_angles: [ 2.8, 0.1, 2.6, 0.1, 2.6, 0.1, 2.6, 1.45] softstop_tolerance: 0.2 - softstop_torque_gains: [65, 10, 10, 5, 5, 1, 1, 0] + softstop_torque_gains: [65, 20, 20, 10, 10, 4, 4, 0] ### Motor configuration motor_names: diff --git a/blue_descriptions/urdf/gripper_v1/gripper.urdf.xacro b/blue_descriptions/urdf/gripper_v1/gripper.urdf.xacro index 5902b3ad..b5c37c29 100644 --- a/blue_descriptions/urdf/gripper_v1/gripper.urdf.xacro +++ b/blue_descriptions/urdf/gripper_v1/gripper.urdf.xacro @@ -103,7 +103,7 @@ type="revolute"> + rpy="-1.5708 -2.4803E-15 -3.1416" /> BlueKinematics::getActuatorCommands( // TODO: hacky and temporary double eps_vel = 0.1; if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance)) { - if(joint_pos_[i] > softstop_max_angles[i] - softstop_tolerance/2.0) { - joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; - } else if (joint_vel_[i] < 0) { - joint_cmd_[i] += -1.0 * softstop_torque_gains[i] * joint_vel_[i]; - } + // if(joint_pos_[i] > softstop_max_angles[i] - softstop_tolerance/2.0) { + // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; + // } else if (joint_vel_[i] < 0) { double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); - joint_cmd_[i] += -0.1 * softstop_torque_gains[i] * pow(offset, 1); + joint_cmd_[i] += -1.0 * offset * softstop_torque_gains[i] * joint_vel_[i]; + // } + joint_cmd_[i] += -0.2 * softstop_torque_gains[i] * pow(offset, 1); // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance)) { - if(joint_pos_[i] < softstop_min_angles[i] + softstop_tolerance/2.0) { - joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; - } else if (joint_vel_[i] > 0) { - joint_cmd_[i] += -1.0 * softstop_torque_gains[i] * joint_vel_[i]; - } + // if(joint_pos_[i] < softstop_min_angles[i] + softstop_tolerance/2.0) { + // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; + // } else if (joint_vel_[i] > 0) { double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; - joint_cmd_[i] += 0.1 * softstop_torque_gains[i] * pow(offset, 1); + joint_cmd_[i] += -1.0 * offset *softstop_torque_gains[i] * joint_vel_[i]; + // } + joint_cmd_[i] += 0.2 * softstop_torque_gains[i] * pow(offset, 1); // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; } From 9893b0ffd7a435990f41dab611cb63dd88d2cf1a Mon Sep 17 00:00:00 2001 From: Brent Yi Date: Wed, 27 Mar 2019 21:56:17 -0500 Subject: [PATCH 6/6] updates --- .../config/robot_parameters_right.yaml | 2 +- .../src/blue_kinematics.cpp | 23 ++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/blue_bringup/config/robot_parameters_right.yaml b/blue_bringup/config/robot_parameters_right.yaml index d313d139..ca08112b 100644 --- a/blue_bringup/config/robot_parameters_right.yaml +++ b/blue_bringup/config/robot_parameters_right.yaml @@ -24,7 +24,7 @@ blue_hardware: softstop_min_angles: [-3.3, -2.25, -2.6, -2.25, -2.6, -2.25, -2.6, -1.45] softstop_max_angles: [ 2.2, 0.5, 2.6, 0.5, 2.6, 0.2, 2.6, 1.45] softstop_tolerance: 0.19 - softstop_torque_limit: 95.0 + softstop_torque_gains: [65, 20, 20, 10, 10, 4, 4, 0] ### Motor configuration motor_names: diff --git a/blue_hardware_interface/src/blue_kinematics.cpp b/blue_hardware_interface/src/blue_kinematics.cpp index 12930174..4da35da7 100644 --- a/blue_hardware_interface/src/blue_kinematics.cpp +++ b/blue_hardware_interface/src/blue_kinematics.cpp @@ -391,24 +391,31 @@ std::vector BlueKinematics::getActuatorCommands( // Soft stops // TODO: hacky and temporary double eps_vel = 0.1; - if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance)) { + if(joint_pos_[i] > (softstop_max_angles[i] - softstop_tolerance) && joint_vel_[i] > 0) { + double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); + double spring = offset * softstop_torque_gains[i]; + double applied = std::min(spring, joint_vel_[i]); + joint_cmd_[i] += -applied; + // if(joint_pos_[i] > softstop_max_angles[i] - softstop_tolerance/2.0) { // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; // } else if (joint_vel_[i] < 0) { - double offset = joint_pos_[i] - (softstop_max_angles[i] - softstop_tolerance); - joint_cmd_[i] += -1.0 * offset * softstop_torque_gains[i] * joint_vel_[i]; + // joint_cmd_[i] += -1.0 * offset * softstop_torque_gains[i] * joint_vel_[i]; // } - joint_cmd_[i] += -0.2 * softstop_torque_gains[i] * pow(offset, 1); + // joint_cmd_[i] += -0.2 * softstop_torque_gains[i] * pow(offset, 1); // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; - } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance)) { + } else if (joint_pos_[i] < (softstop_min_angles[i] + softstop_tolerance) && joint_vel_[i] < 0) { + double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; + double spring = offset * softstop_torque_gains[i]; + double applied = std::min(spring, -joint_vel_[i]); + joint_cmd_[i] += applied; // if(joint_pos_[i] < softstop_min_angles[i] + softstop_tolerance/2.0) { // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; // } else if (joint_vel_[i] > 0) { - double offset = (softstop_min_angles[i] + softstop_tolerance) - joint_pos_[i]; - joint_cmd_[i] += -1.0 * offset *softstop_torque_gains[i] * joint_vel_[i]; + // joint_cmd_[i] += -1.0 *offset *softstop_torque_gains[i] * joint_vel_[i]; // } - joint_cmd_[i] += 0.2 * softstop_torque_gains[i] * pow(offset, 1); + // joint_cmd_[i] += 0.2 * softstop_torque_gains[i] * pow(offset, 1); // joint_cmd_[i] += -0.5 * softstop_torque_gains[i] * joint_vel_[i]; }