diff --git a/blue_bringup/config/robot_parameters_left.yaml b/blue_bringup/config/robot_parameters_left.yaml
index 7dd2ac29..48da6b64 100644
--- a/blue_bringup/config/robot_parameters_left.yaml
+++ b/blue_bringup/config/robot_parameters_left.yaml
@@ -22,9 +22,9 @@ 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: [-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_torque_limit: 95.0
+ 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, 20, 20, 10, 10, 4, 4, 0]
### Motor configuration
motor_names:
diff --git a/blue_bringup/config/robot_parameters_right.yaml b/blue_bringup/config/robot_parameters_right.yaml
index e1b5395c..ca08112b 100644
--- a/blue_bringup/config/robot_parameters_right.yaml
+++ b/blue_bringup/config/robot_parameters_right.yaml
@@ -22,9 +22,9 @@ 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
+ 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" />
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 caa451c3..4da35da7 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) {
@@ -385,23 +385,43 @@ 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;
- 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);
+ double eps_vel = 0.1;
+ 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) {
+ // 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) && 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) {
+ // 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];
}
+
+ // 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