Skip to content

Commit 4428513

Browse files
ayin21ayin21
authored
Merge branch 'main' into feature/better_ball_search
2 parents 089be79 + 12a0d5d commit 4428513

File tree

16 files changed

+102
-49
lines changed

16 files changed

+102
-49
lines changed

‎bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,7 @@ $IsPenalized
158158
READY --> $AnyGoalScoreRecently + time:50
159159
YES --> #PositioningReady
160160
NO --> $DoOnce
161-
NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:1, @GetWalkready + r:false
161+
NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
162162
DONE --> #PositioningReady
163163
SET --> $SecondaryStateDecider
164164
PENALTYSHOOT --> $SecondaryStateTeamDecider

‎bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@ find_package(controller_manager REQUIRED)
1616
find_package(diagnostic_msgs REQUIRED)
1717
find_package(dynamixel_workbench_toolbox REQUIRED)
1818
find_package(hardware_interface REQUIRED)
19+
find_package(moveit_core REQUIRED)
20+
find_package(moveit_ros_planning REQUIRED)
1921
find_package(pluginlib REQUIRED)
2022
find_package(rclcpp REQUIRED)
2123
find_package(realtime_tools REQUIRED)
@@ -60,6 +62,8 @@ ament_target_dependencies(
6062
diagnostic_msgs
6163
dynamixel_workbench_toolbox
6264
hardware_interface
65+
moveit_core
66+
moveit_ros_planning
6367
pluginlib
6468
rclcpp
6569
realtime_tools

‎bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,8 @@ class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInte
7070
unsigned int joint_count_;
7171

7272
std::vector<std::string> joint_names_;
73+
std::vector<double> lower_joint_limits_;
74+
std::vector<double> upper_joint_limits_;
7375

7476
std::vector<double> goal_position_;
7577
std::vector<double> goal_effort_;

‎bitbots_lowlevel/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp‎

Lines changed: 26 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,6 @@
1+
#include <moveit/robot_model/robot_model.h>
2+
#include <moveit/robot_model_loader/robot_model_loader.h>
3+
14
#include <bitbots_ros_control/dynamixel_servo_hardware_interface.hpp>
25
#include <bitbots_ros_control/utils.hpp>
36
#include <utility>
@@ -63,6 +66,18 @@ bool DynamixelServoHardwareInterface::init() {
6366
joint_map_[joint_names_[i]] = i;
6467
}
6568

69+
// read lower and upper limits
70+
robot_model_loader::RobotModelLoader rml(nh_, "robot_description", false);
71+
moveit::core::RobotModelPtr model = rml.getModel();
72+
lower_joint_limits_.resize(joint_count_);
73+
upper_joint_limits_.resize(joint_count_);
74+
for (const std::string &joint_name : joint_names_) {
75+
moveit::core::JointModel *jm = model->getJointModel(joint_name);
76+
// we use getVariableBounds()[0] because there is only a single variable for all of our joints
77+
lower_joint_limits_[joint_map_[joint_name]] = jm->getVariableBounds()[0].min_position_;
78+
upper_joint_limits_[joint_map_[joint_name]] = jm->getVariableBounds()[0].max_position_;
79+
}
80+
6681
std::string control_mode;
6782
control_mode = nh_->get_parameter("servos.control_mode").as_string();
6883
RCLCPP_INFO(nh_->get_logger(), "Control mode: %s", control_mode.c_str());
@@ -88,10 +103,19 @@ void DynamixelServoHardwareInterface::commandCb(const bitbots_msgs::msg::JointCo
88103
RCLCPP_ERROR(nh_->get_logger(), "Dynamixel Controller got command with inconsistent array lengths.");
89104
return;
90105
}
91-
// todo improve performance
92106
for (unsigned int i = 0; i < command_msg.joint_names.size(); i++) {
93107
int joint_id = joint_map_[command_msg.joint_names[i]];
94-
goal_position_[joint_id] = command_msg.positions[i];
108+
if (command_msg.positions[i] > upper_joint_limits_[joint_id] ||
109+
command_msg.positions[i] < lower_joint_limits_[joint_id]) {
110+
RCLCPP_WARN_STREAM(nh_->get_logger(), "Invalid position for " << command_msg.joint_names[i] << ": "
111+
<< command_msg.positions[i] << " not in ("
112+
<< lower_joint_limits_[joint_id] << ", "
113+
<< upper_joint_limits_[joint_id] << ")");
114+
goal_position_[joint_id] =
115+
std::clamp(command_msg.positions[i], lower_joint_limits_[joint_id], upper_joint_limits_[joint_id]);
116+
} else {
117+
goal_position_[joint_id] = command_msg.positions[i];
118+
}
95119
goal_velocity_[joint_id] = command_msg.velocities[i];
96120
goal_acceleration_[joint_id] = command_msg.accelerations[i];
97121
goal_effort_[joint_id] = command_msg.max_currents[i];

‎bitbots_misc/bitbots_docs/docs/manual/tutorials/extrinsic_calibration.rst‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,8 @@ Setup
1414
1. Start the visualization launch-file including the necessary motion and vision nodes.
1515

1616
.. code-block:: bash
17-
ros2 launch bitbots_extrinsic_calibration viz_extrinsic_calibration.launch
17+
18+
ros2 launch bitbots_extrinsic_calibration viz_extrinsic_calibration.launch
1819
1920
2. In Dynamic Reconfigure open the parameters (left panel) for the nodes: :code:`bitbots_extrinsic_imu_calibration` and :code:`bitbots_extrinsic_camera_calibration`.
2021

@@ -26,6 +27,10 @@ Do the calibration
2627

2728
2. Open rqt and navigate to **Plugins** > **Configurations** > **Dynamic Reconfigure** where you can configure the parameters.
2829

30+
3. Place the robot outside the field exactly in front of the middle line.
31+
32+
4. Use the :code:`2D Pose Estimate` button in RViz to place the virtual robot in the corresponding pose.
33+
2934
.. note::
3035
If you change the calibration first change all parameters to :code:`0.0`.
3136
Then start with the adjustment of the IMU parameters.

‎bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/bitbots_extrinsic_camera_calibration:
22
ros__parameters:
3-
offset_x: -0.025
3+
offset_x: -0.02
44
offset_y: 0.0
55
offset_z: 0.0
66

‎bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
11
/bitbots_extrinsic_camera_calibration:
22
ros__parameters:
3-
offset_x: -0.07
4-
offset_y: 0.0
3+
offset_x: -0.06
4+
offset_y: 0.01
55
offset_z: 0.0
66

77
/bitbots_extrinsic_imu_calibration:
88
ros__parameters:
9-
offset_x: -0.0
10-
offset_y: 0.01
9+
offset_x: 0.0
10+
offset_y: 0.017
1111
offset_z: 0.0

‎bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,8 @@ walking:
44
ros__parameters:
55
engine:
66
trunk_x_offset: 0.0
7+
trunk_y_offset: -0.01
8+
79
node:
810
x_bias: -0.005
911
y_bias: -0.008

‎bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@ walking:
77
foot_apex_phase: 0.5
88
foot_apex_pitch: 0.0
99
foot_distance: 0.2
10-
foot_overshoot_phase: 0.67
11-
foot_overshoot_ratio: 0.1
10+
foot_overshoot_phase: 0.92
11+
foot_overshoot_ratio: 0.0
1212
foot_put_down_phase: 1.0
1313
foot_put_down_z_offset: 0.0
1414
foot_rise: 0.04
@@ -26,7 +26,7 @@ walking:
2626
trunk_pitch_p_coef_forward: 0.0
2727
trunk_pitch_p_coef_turn: 0.0
2828
trunk_swing: 0.25
29-
trunk_x_offset: -0.02
29+
trunk_x_offset: -0.01
3030
trunk_x_offset_p_coef_forward: 0.1
3131
trunk_x_offset_p_coef_turn: 0.0
3232
trunk_y_offset: 0.0
@@ -74,11 +74,11 @@ walking:
7474
trunk_pid:
7575
pitch:
7676
antiwindup: false
77-
d: 0.0
77+
d: 0.004
7878
i: 0.0
7979
i_clamp_max: 0.0
8080
i_clamp_min: 0.0
81-
p: 0.0
81+
p: 0.0035
8282
roll:
8383
antiwindup: false
8484
d: 0.0

‎bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ void WalkStabilizer::reset() {
1515
pid_trunk_fused_roll_.reset();
1616
}
1717

18-
WalkResponse WalkStabilizer::stabilize(const WalkResponse &response, const rclcpp::Duration &dt) {
18+
WalkResponse WalkStabilizer::stabilize(const WalkResponse& response, const rclcpp::Duration& dt) {
1919
// compute orientation with PID control
2020
double goal_pitch, goal_roll, goal_yaw;
2121
tf2::Matrix3x3(response.support_foot_to_trunk.getRotation()).getRPY(goal_roll, goal_pitch, goal_yaw);
@@ -33,14 +33,15 @@ WalkResponse WalkStabilizer::stabilize(const WalkResponse &response, const rclcp
3333
double fused_pitch_correction =
3434
pid_trunk_fused_pitch_.computeCommand(goal_fused.fusedPitch - response.current_fused_pitch, dt);
3535

36-
tf2::Quaternion corrected_orientation;
37-
goal_fused.fusedRoll += fused_roll_correction;
38-
goal_fused.fusedPitch += fused_pitch_correction;
39-
Eigen::Quaterniond goal_orientation_eigen_corrected = rot_conv::QuatFromFused(goal_fused);
40-
tf2::convert(goal_orientation_eigen_corrected, corrected_orientation);
36+
// Change trunk x offset (in the trunks frame of reference) based on the PID controllers
37+
WalkResponse stabilized_response{response};
4138

42-
WalkResponse stabilized_response = response;
43-
stabilized_response.support_foot_to_trunk.setRotation(corrected_orientation);
39+
tf2::Transform trunk_offset;
40+
trunk_offset.setOrigin(tf2::Vector3(fused_pitch_correction, fused_roll_correction, 0.0));
41+
trunk_offset.setRotation(tf2::Quaternion::getIdentity());
42+
43+
// apply the trunk offset to the trunk pose
44+
stabilized_response.support_foot_to_trunk = trunk_offset * response.support_foot_to_trunk;
4445

4546
return stabilized_response;
4647
}

0 commit comments

Comments
 (0)