openarm/openarm_teleop
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加多关节传感器控制
Some checks failed
Test / Lint (push) Has been cancelled
Details
Test / Build (ubuntu-22.04) (push) Has been cancelled
Details

Browse Source
This commit is contained in:
shen 2026-03-26 20:12:20 +08:00
parent 80fca8524c
commit 1fb3f08f9a
1 changed files with 124 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

173
control/openarm_joint_sensor.cpp
View File

@ -13,6 +13,9 @@
// limitations under the License. // limitations under the License.
#include <atomic> #include <atomic>
#include <array>
#include <algorithm>
#include <cmath>
#include <chrono> #include <chrono>
#include <controller/control.hpp> #include <controller/control.hpp>
#include <controller/dynamics.hpp> #include <controller/dynamics.hpp>
@ -35,15 +38,58 @@
#include <yamlloader.hpp> #include <yamlloader.hpp>
std::atomic<bool> keep_running(true); std::atomic<bool> keep_running(true);
std::atomic<double> sensor_angle_rad(0.0); constexpr std::size_t SENSOR_JOINT_NUM = 7;
std::atomic<double> sensor_velocity_rad_s(0.0); std::array<std::atomic<double>, SENSOR_JOINT_NUM> sensor_angle_rad;
std::atomic<bool> sensor_angle_ready(false); std::array<std::atomic<double>, SENSOR_JOINT_NUM> sensor_velocity_rad_s;
std::array<std::atomic<bool>, SENSOR_JOINT_NUM> sensor_angle_ready;
constexpr canid_t SENSOR_CAN_ID = 0x01; // TODO: replace with your sensor CAN ID struct openarm_joint_sensor_status
constexpr double SENSOR_SCALE = 1e-3; // TODO: replace with your sensor scaling {
constexpr double SENSOR_OFFSET = 0.0; uint8_t id;
double angle_rad;
double velocity_rad_s;
double torque_Nm;
};
constexpr std::array<canid_t, SENSOR_JOINT_NUM> SENSOR_CAN_ID = {0x01, 0x02, 0x03, 0x04,
0x05, 0x06, 0x07};
// Per-joint sign: use +1.0 or -1.0 to match each joint's positive rotation direction.
constexpr std::array<double, SENSOR_JOINT_NUM> SENSOR_DIRECTION_SIGN = {
-1.0, 1.0, -1.0, 1.0, 1.0, 1.0, 1.0};
// Per-joint zero offset in radians: joint_angle = sign * raw_angle + offset.
constexpr std::array<double, SENSOR_JOINT_NUM> SENSOR_ZERO_OFFSET_RAD = {
2.75, -2.49, 0.08, 0.0, 0.0, 0.0, 0.0};
// constexpr double SENSOR_SCALE = 1e-3; // TODO: replace with your sensor scaling
// constexpr double SENSOR_OFFSET = 0.0;
constexpr double SENSOR_FILTER_ALPHA = 0.05; // 0~1, larger = less smoothing constexpr double SENSOR_FILTER_ALPHA = 0.05; // 0~1, larger = less smoothing
constexpr double SENSOR_VEL_FILTER_ALPHA = 0.2; // 0~1, larger = less smoothing constexpr double SENSOR_VEL_FILTER_ALPHA = 0.2; // 0~1, larger = less smoothing
constexpr double pi = 3.14159265358979323846;
constexpr double TWO_PI = 2.0 * pi;
double wrap_to_pi(double angle_rad) {
return std::atan2(std::sin(angle_rad), std::cos(angle_rad));
}
double convert_0_to_2pi_to_signed(double angle_rad_0_to_2pi) {
double angle = std::fmod(angle_rad_0_to_2pi, TWO_PI);
if (angle < 0.0) {
angle += TWO_PI;
}
if (angle > pi) {
angle -= TWO_PI;
}
return angle;
}
int sensor_id_to_joint_index(canid_t id) {
for (std::size_t i = 0; i < SENSOR_CAN_ID.size(); ++i) {
if (SENSOR_CAN_ID[i] == id) {
return static_cast<int>(i);
}
}
return -1;
}
int open_can_socket(const std::string &can_interface) { int open_can_socket(const std::string &can_interface) {
int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW); int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
@ -69,8 +115,9 @@ int open_can_socket(const std::string &can_interface) {
} }
struct can_filter filter; struct can_filter filter;
filter.can_id = SENSOR_CAN_ID; filter.can_id = 0x01;
filter.can_mask = CAN_SFF_MASK; filter.can_mask = CAN_SFF_MASK & 0xF0;
// filter.can_mask = CAN_SFF_MASK;
if (setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { if (setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
close(sock); close(sock);
throw std::runtime_error("Failed to set CAN filter"); throw std::runtime_error("Failed to set CAN filter");
@ -79,7 +126,7 @@ int open_can_socket(const std::string &can_interface) {
return sock; return sock;
} }
bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) { bool read_sensor(int sock, openarm_joint_sensor_status &status_out) {
struct pollfd pfd; struct pollfd pfd;
pfd.fd = sock; pfd.fd = sock;
pfd.events = POLLIN; pfd.events = POLLIN;
@ -95,7 +142,15 @@ bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) {
} }
const canid_t frame_id = frame.can_id & CAN_SFF_MASK; const canid_t frame_id = frame.can_id & CAN_SFF_MASK;
if (frame_id != SENSOR_CAN_ID || frame.can_dlc < 4) { bool valid_id = false;
for (const auto &id : SENSOR_CAN_ID) {
if (frame_id == id) {
valid_id = true;
break;
}
// std::cout << "Received CAN ID: " << frame_id << ", expected: " << id << std::endl;
}
if (!valid_id || frame.can_dlc < 5) {
return false; return false;
} }
@ -106,10 +161,12 @@ bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) {
rawVel |= static_cast<int32_t>(frame.data[3]) << 8; rawVel |= static_cast<int32_t>(frame.data[3]) << 8;
rawVel |= static_cast<int32_t>(frame.data[4]); rawVel |= static_cast<int32_t>(frame.data[4]);
velocity_rad_s_out = (static_cast<double>(rawVel) / 100.0 / 4096.0) * 360.0 * (M_PI / 180.0); // Example conversion, adjust as needed status_out.id = frame_id;
status_out.velocity_rad_s = (static_cast<double>(rawVel) / 100.0 / 4096.0) * 360.0 * (pi / 180.0);
angle_rad_out = (static_cast<double>(rawPos) / 4096.0) * 360.0; // Example conversion, adjust as needed // Sensor reports angle in [0, 2pi). Keep it as-is here and normalize later.
angle_rad_out = angle_rad_out * (M_PI / 180.0) - 3.14; status_out.angle_rad = (static_cast<double>(rawPos) / 4096.0) * TWO_PI;
std::cout << "CAN ID: " << frame_id << " angle: " << status_out.angle_rad << " velocity: " << status_out.velocity_rad_s << std::endl;
return true; return true;
} }
@ -123,8 +180,9 @@ void signal_handler(int signal) {
class FollowerArmThread : public PeriodicTimerThread { class FollowerArmThread : public PeriodicTimerThread {
public: public:
FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f, FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f,
double hz = 500.0) std::size_t joint_num, double hz = 500.0)
: PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f),
joint_num_(joint_num) {}
protected: protected:
void before_start() override { std::cout << "follower start thread " << std::endl; } void before_start() override { std::cout << "follower start thread " << std::endl; }
@ -133,14 +191,15 @@ protected:
void on_timer() override { void on_timer() override {
static auto prev_time = std::chrono::steady_clock::now(); static auto prev_time = std::chrono::steady_clock::now();
if (sensor_angle_ready.load()) { for (std::size_t i = 0; i < joint_num_; ++i) {
// control_f_->SetArmJointReference(0, 0, 0.01); if (!sensor_angle_ready[i].load()) {
control_f_->SetArmJointReference(0, sensor_angle_rad.load(), continue;
sensor_velocity_rad_s.load()); }
std::cout << "Sensor angle (rad): " << sensor_angle_rad.load() << std::endl; control_f_->SetArmJointReference(static_cast<int>(i), sensor_angle_rad[i].load(),
std::cout << "Sensor velocity (rad/s): " << sensor_velocity_rad_s.load() << std::endl; sensor_velocity_rad_s[i].load());
// std::cout << "Joint " << i << " angle: " << sensor_angle_rad[i].load()
// << " velocity: " << sensor_velocity_rad_s[i].load() << std::endl;
} }
// control_f_->SetArmJointReference(0, 0.2, 0.01); // Example: set joint 0 to 0 radians
control_f_->unilateral_step(); control_f_->unilateral_step();
@ -159,12 +218,18 @@ protected:
private: private:
std::shared_ptr<RobotSystemState> robot_state_; std::shared_ptr<RobotSystemState> robot_state_;
Control *control_f_; Control *control_f_;
std::size_t joint_num_;
}; };
int main(int argc, char **argv) { int main(int argc, char **argv) {
try { try {
std::signal(SIGINT, signal_handler); std::signal(SIGINT, signal_handler);
for (std::size_t i = 0; i < SENSOR_JOINT_NUM; ++i) {
sensor_angle_rad[i].store(0.0);
sensor_velocity_rad_s[i].store(0.0);
sensor_angle_ready[i].store(false);
}
// default configration // default configration
std::string arm_side = "right_arm"; std::string arm_side = "right_arm";
@ -232,41 +297,40 @@ int main(int argc, char **argv) {
int sensor_can_socket = open_can_socket(sensor_can_interface); int sensor_can_socket = open_can_socket(sensor_can_interface);
std::thread can_read_thread([&]() { std::thread can_read_thread([&]() {
double filtered_angle = 0.0; std::array<double, SENSOR_JOINT_NUM> filtered_angle{};
bool filter_initialized = false; std::array<double, SENSOR_JOINT_NUM> filtered_velocity{};
double filtered_velocity = 0.0; std::array<bool, SENSOR_JOINT_NUM> filter_initialized{};
double prev_angle = 0.0; openarm_joint_sensor_status status;
auto prev_time = std::chrono::steady_clock::now();
double velocity = 0.0;
while (keep_running) { while (keep_running) {
double angle = 0.0; if (read_sensor(sensor_can_socket, status)) {
if (read_sensor(sensor_can_socket, angle, velocity)) { const int joint_idx = sensor_id_to_joint_index(status.id);
auto now = std::chrono::steady_clock::now(); if (joint_idx < 0) {
double dt = continue;
std::chrono::duration_cast<std::chrono::duration<double>>(now - prev_time) }
.count();
// std::cout << "dt: " << dt << " s, raw angle: " << angle << " rad" << std::endl;
prev_time = now;
if (!filter_initialized) { const std::size_t i = static_cast<std::size_t>(joint_idx);
filtered_angle = angle; const double sensor_angle_signed = convert_0_to_2pi_to_signed(status.angle_rad);
filter_initialized = true; const double angle = wrap_to_pi(
prev_angle = angle; SENSOR_DIRECTION_SIGN[i] * sensor_angle_signed + SENSOR_ZERO_OFFSET_RAD[i]);
filtered_velocity = 0.0; const double velocity = SENSOR_DIRECTION_SIGN[i] * status.velocity_rad_s;
if (!filter_initialized[i]) {
filtered_angle[i] = angle;
filtered_velocity[i] = velocity;
filter_initialized[i] = true;
} else { } else {
filtered_angle = SENSOR_FILTER_ALPHA * angle + filtered_angle[i] = SENSOR_FILTER_ALPHA * angle +
(1.0 - SENSOR_FILTER_ALPHA) * filtered_angle; (1.0 - SENSOR_FILTER_ALPHA) * filtered_angle[i];
filtered_velocity[i] = SENSOR_VEL_FILTER_ALPHA * velocity +
if (dt > 1e-6) {
filtered_velocity = SENSOR_VEL_FILTER_ALPHA * velocity +
(1.0 - SENSOR_VEL_FILTER_ALPHA) * (1.0 - SENSOR_VEL_FILTER_ALPHA) *
filtered_velocity; filtered_velocity[i];
} }
prev_angle = angle;
} sensor_angle_rad[i].store(filtered_angle[i]);
sensor_angle_rad.store(filtered_angle); sensor_velocity_rad_s[i].store(filtered_velocity[i]);
sensor_velocity_rad_s.store(filtered_velocity); sensor_angle_ready[i].store(true);
sensor_angle_ready.store(true); std::cout << "Joint " << i << " filtered_angle: " << filtered_angle[i]
<< ", filtered_velocity: " << filtered_velocity[i] << std::endl;
} }
} }
}); });
@ -318,7 +382,10 @@ int main(int argc, char **argv) {
// thread_f.join(); // thread_f.join();
// Start control process // Start control process
FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); const std::size_t controlled_joint_num =
std::min<std::size_t>(follower_arm_motor_num, SENSOR_JOINT_NUM);
FollowerArmThread follower_thread(follower_state, control_follower, controlled_joint_num,
FREQUENCY);
follower_thread.start_thread(); follower_thread.start_thread();