Skip to content

Commit

Permalink
Added Velocity feedback to UR
Browse files Browse the repository at this point in the history 
* Added virtual interface function to RobotInterface
* Added getCartesianVelocity in URRTDE
* Added velocity buffer/mtx/timestamp/timestamp buffer/getters in
  ManipServer, update loops to populate the buffers

Test: passed compiling
  • Loading branch information
@yifan-hou
yifan-hou committed Dec 27, 2024
1 parent 311ea7d commit 3ce8cb5
Show file tree
Hide file tree
Showing 6 changed files with 70 additions and 1 deletion.
2 changes: 2 additions & 0 deletions hardware/ur_rtde/include/ur_rtde/ur_rtde.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ class URRTDE : public RobotInterfaces {
bool init(RUT::TimePoint time0, const URRTDEConfig& config);

bool getCartesian(RUT::Vector7d& pose) override;
bool getCartesianVelocity(RUT::Vector6d& velocity) override;

bool setCartesian(const RUT::Vector7d& pose) override;

// interfaces unique to URRTDE
Expand Down
17 changes: 17 additions & 0 deletions hardware/ur_rtde/src/ur_rtde.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@ struct URRTDE::Implementation {

std::vector<double>
tcp_pose_feedback{}; // std vector to be compatible with ur_rtde lib
std::vector<double>
tcp_speed_feedback{}; // std vector to be compatible with ur_rtde lib
std::vector<double> tcp_pose_command{};
RUT::Vector7d pose_xyzq_set_prev{};

Expand All @@ -34,6 +36,7 @@ struct URRTDE::Implementation {

bool initialize(RUT::TimePoint time0, const URRTDE::URRTDEConfig& config);
bool getCartesian(RUT::Vector7d& pose_xyzq);
bool getCartesianVelocity(RUT::Vector6d& vel);
bool getWrenchBaseOnTool(RUT::Vector6d& wrench);
bool getWrenchTool(RUT::Vector6d& wrench);
bool checkCartesianTarget(RUT::Vector7d& pose_xyzq_set);
Expand All @@ -45,6 +48,7 @@ struct URRTDE::Implementation {

URRTDE::Implementation::Implementation() {
tcp_pose_feedback.resize(6);
tcp_speed_feedback.resize(6);
tcp_pose_command.resize(6);
}

Expand Down Expand Up @@ -134,6 +138,15 @@ bool URRTDE::Implementation::getCartesian(RUT::Vector7d& pose_xyzq) {
return true;
}

bool URRTDE::Implementation::getCartesianVelocity(RUT::Vector6d& velocity) {
tcp_speed_feedback = rtde_receive_ptr->getActualTCPSpeed(); // std vector

for (int i = 0; i < 6; i++) {
velocity[i] = tcp_speed_feedback[i];
}
return true;
}

bool URRTDE::Implementation::getWrenchBaseOnTool(RUT::Vector6d& wrench) {
std::vector<double> wrench_bot_vec = rtde_receive_ptr->getActualTCPForce();
wrench = RUT::Vector6d::Map(wrench_bot_vec.data(), 6);
Expand Down Expand Up @@ -282,6 +295,10 @@ bool URRTDE::getCartesian(RUT::Vector7d& pose_xyzq) {
return m_impl->getCartesian(pose_xyzq);
}

bool URRTDE::getCartesianVelocity(RUT::Vector6d& velocity) {
return m_impl->getCartesianVelocity(velocity);
}

bool URRTDE::getWrenchBaseOnTool(RUT::Vector6d& wrench) {
return m_impl->getWrenchBaseOnTool(wrench);
}
Expand Down
9 changes: 9 additions & 0 deletions include/hardware_interfaces/robot_interfaces.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,15 @@ class RobotInterfaces {
*/
virtual bool setJoints(const Eigen::VectorXd& joints) = 0;

// ----------------------------------------
// Optional interfaces
// ----------------------------------------
virtual bool getCartesianVelocity(RUT::Vector6d& velocity) {
std::cerr << "[RobotInterfaces] getCartesianVelocity not implemented yet"
<< std::endl;
return false;
}

// ----------------------------------------
// public state and parameters
// ----------------------------------------
Expand Down
8 changes: 8 additions & 0 deletions workcell/table_top_manip/include/table_top_manip/manip_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,8 @@ class ManipServer {
const Eigen::MatrixXd get_camera_rgb(int k, int camera_id = 0);
const Eigen::MatrixXd get_wrench(int k, int sensor_id = 0);
const Eigen::MatrixXd get_pose(int k, int robot_id = 0);
const Eigen::MatrixXd get_vel(int k, int robot_id = 0);
const int get_test();
const Eigen::MatrixXd get_eoat(int k, int robot_id = 0);

// the following functions return the timestamps of
Expand All @@ -126,6 +128,8 @@ class ManipServer {
const Eigen::VectorXd get_camera_rgb_timestamps_ms(int id = 0);
const Eigen::VectorXd get_wrench_timestamps_ms(int id = 0);
const Eigen::VectorXd get_pose_timestamps_ms(int id = 0);
const Eigen::VectorXd get_vel_timestamps_ms(int id = 0);
const double get_test_timestamp_ms();
const Eigen::VectorXd get_eoat_timestamps_ms(int id = 0);

double get_timestamp_now_ms(); // access the current hardware time
Expand Down Expand Up @@ -166,6 +170,7 @@ class ManipServer {
// data buffer
std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _camera_rgb_buffers;
std::vector<RUT::DataBuffer<Eigen::VectorXd>> _pose_buffers;
std::vector<RUT::DataBuffer<Eigen::VectorXd>> _vel_buffers;
std::vector<RUT::DataBuffer<Eigen::VectorXd>> _eoat_buffers;
std::vector<RUT::DataBuffer<Eigen::VectorXd>> _wrench_buffers;
// action buffer
Expand All @@ -175,6 +180,7 @@ class ManipServer {

std::vector<RUT::DataBuffer<double>> _camera_rgb_timestamp_ms_buffers;
std::vector<RUT::DataBuffer<double>> _pose_timestamp_ms_buffers;
std::vector<RUT::DataBuffer<double>> _vel_timestamp_ms_buffers;
std::vector<RUT::DataBuffer<double>> _eoat_timestamp_ms_buffers;
std::vector<RUT::DataBuffer<double>> _wrench_timestamp_ms_buffers;
std::vector<RUT::DataBuffer<double>> _waypoints_timestamp_ms_buffers;
Expand All @@ -183,6 +189,7 @@ class ManipServer {

std::deque<std::mutex> _camera_rgb_buffer_mtxs;
std::deque<std::mutex> _pose_buffer_mtxs;
std::deque<std::mutex> _vel_buffer_mtxs;
std::deque<std::mutex> _eoat_buffer_mtxs;
std::deque<std::mutex> _wrench_buffer_mtxs;
std::deque<std::mutex> _waypoints_buffer_mtxs;
Expand Down Expand Up @@ -255,6 +262,7 @@ class ManipServer {
// temp variables storing timestamps of data just being fetched
std::vector<Eigen::VectorXd> _camera_rgb_timestamps_ms;
std::vector<Eigen::VectorXd> _pose_timestamps_ms;
std::vector<Eigen::VectorXd> _vel_timestamps_ms;
std::vector<Eigen::VectorXd> _eoat_timestamps_ms;
std::vector<Eigen::VectorXd> _wrench_timestamps_ms;

Expand Down
26 changes: 25 additions & 1 deletion workcell/table_top_manip/src/manip_server.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,6 +228,7 @@ bool ManipServer::initialize(const std::string& config_path) {
int num_ft_sensors = force_sensor_ptrs[id]->getNumSensors();
_camera_rgb_buffers.push_back(RUT::DataBuffer<Eigen::MatrixXd>());
_pose_buffers.push_back(RUT::DataBuffer<Eigen::VectorXd>());
_vel_buffers.push_back(RUT::DataBuffer<Eigen::VectorXd>());
_eoat_buffers.push_back(RUT::DataBuffer<Eigen::VectorXd>());
_wrench_buffers.push_back(RUT::DataBuffer<Eigen::VectorXd>());
_waypoints_buffers.push_back(RUT::DataBuffer<Eigen::VectorXd>());
Expand All @@ -236,6 +237,7 @@ bool ManipServer::initialize(const std::string& config_path) {

_camera_rgb_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
_pose_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
_vel_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
_eoat_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
_wrench_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
_waypoints_timestamp_ms_buffers.push_back(RUT::DataBuffer<double>());
Expand All @@ -246,8 +248,11 @@ bool ManipServer::initialize(const std::string& config_path) {
_config.rgb_buffer_size, 3 * _config.output_rgb_hw[0],
_config.output_rgb_hw[1], "camera_rgb" + std::to_string(id));

_pose_buffers[id].initialize(_config.pose_buffer_size, 7, 1, // xyzqwqxqyqz
_pose_buffers[id].initialize(_config.pose_buffer_size, 7,
1, //xyz qwqxqyqz
"pose" + std::to_string(id));
_vel_buffers[id].initialize(_config.pose_buffer_size, 6, 1, // xyz rxryrz
"vel" + std::to_string(id));
_eoat_buffers[id].initialize(_config.eoat_buffer_size, 2, 1, // pos, force
"eoat" + std::to_string(id));
_wrench_buffers[id].initialize(_config.wrench_buffer_size,
Expand All @@ -267,6 +272,9 @@ bool ManipServer::initialize(const std::string& config_path) {
_pose_timestamp_ms_buffers[id].initialize(
_config.pose_buffer_size, 1, 1,
"pose" + std::to_string(id) + "_timestamp_ms");
_vel_timestamp_ms_buffers[id].initialize(
_config.pose_buffer_size, 1, 1, // pose/vel buffers have the same size
"vel" + std::to_string(id) + "_timestamp_ms");
_eoat_timestamp_ms_buffers[id].initialize(
_config.eoat_buffer_size, 1, 1,
"eoat" + std::to_string(id) + "_timestamp_ms");
Expand All @@ -285,6 +293,7 @@ bool ManipServer::initialize(const std::string& config_path) {
for (int id : _id_list) {
_camera_rgb_buffer_mtxs.emplace_back();
_pose_buffer_mtxs.emplace_back();
_vel_buffer_mtxs.emplace_back();
_eoat_buffer_mtxs.emplace_back();
_wrench_buffer_mtxs.emplace_back();
_waypoints_buffer_mtxs.emplace_back();
Expand Down Expand Up @@ -322,6 +331,7 @@ bool ManipServer::initialize(const std::string& config_path) {
_wrench_fb_mtxs.emplace_back();
_camera_rgb_timestamps_ms.push_back(Eigen::VectorXd());
_pose_timestamps_ms.push_back(Eigen::VectorXd());
_vel_timestamps_ms.push_back(Eigen::VectorXd());
_eoat_timestamps_ms.push_back(Eigen::VectorXd());
_wrench_timestamps_ms.push_back(Eigen::VectorXd());
}
Expand Down Expand Up @@ -497,6 +507,17 @@ const Eigen::MatrixXd ManipServer::get_pose(int k, int id) {
return _pose_buffers[id].get_last_k(k);
}

const Eigen::MatrixXd ManipServer::get_vel(int k, int id) {
std::lock_guard<std::mutex> lock(_vel_buffer_mtxs[id]);
_vel_timestamps_ms[id] = _vel_timestamp_ms_buffers[id].get_last_k(k);
return _vel_buffers[id].get_last_k(k);
}

const int ManipServer::get_test() {
_test_timestamp_ms = _timer.toc_ms();
return 0;
}

const Eigen::VectorXd ManipServer::get_camera_rgb_timestamps_ms(int id) {
return _camera_rgb_timestamps_ms[id];
}
Expand All @@ -506,6 +527,9 @@ const Eigen::VectorXd ManipServer::get_wrench_timestamps_ms(int id) {
const Eigen::VectorXd ManipServer::get_pose_timestamps_ms(int id) {
return _pose_timestamps_ms[id];
}
const Eigen::VectorXd ManipServer::get_vel_timestamps_ms(int id) {
return _vel_timestamps_ms[id];
}
const Eigen::VectorXd ManipServer::get_eoat_timestamps_ms(int id) {
return _eoat_timestamps_ms[id];
}
Expand Down
9 changes: 9 additions & 0 deletions workcell/table_top_manip/src/manip_server_loops.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,13 @@ void ManipServer::robot_loop(const RUT::TimePoint& time0, int id) {

RUT::Vector7d pose_fb;
RUT::Vector7d pose_target_waypoint;
RUT::Vector6d vel_fb;
RUT::Vector7d force_control_ref_pose;
RUT::Vector7d pose_rdte_cmd;
// The following two initial values are used in mock hardware mode
pose_fb << id, 0, 0, 1, 0, 0, 0;
pose_rdte_cmd = pose_fb;
vel_fb << 0, 0, 0, 0, 0, 0;

RUT::Vector6d wrench_fb_ur, wrench_WTr;
RUT::Matrix6d stiffness;
Expand Down Expand Up @@ -63,6 +65,7 @@ void ManipServer::robot_loop(const RUT::TimePoint& time0, int id) {
// real hardware
t_start = urrtde_ptr->rtde_init_period();
urrtde_ptr->getCartesian(pose_fb);
urrtde_ptr->getCartesianVelocity(vel_fb);
time_now_ms = timer.toc_ms();
loop_profiler.stop("compute");
loop_profiler.start();
Expand All @@ -78,6 +81,7 @@ void ManipServer::robot_loop(const RUT::TimePoint& time0, int id) {
// mock hardware
time_now_ms = timer.toc_ms();
pose_fb = pose_rdte_cmd;
vel_fb.setZero();
wrench_fb_ur.setZero();
}
// buffer robot pose
Expand All @@ -88,6 +92,11 @@ void ManipServer::robot_loop(const RUT::TimePoint& time0, int id) {
_pose_buffers[id].put(pose_fb);
_pose_timestamp_ms_buffers[id].put(time_now_ms);
}
{
std::lock_guard<std::mutex> lock(_vel_buffer_mtxs[id]);
_vel_buffers[id].put(vel_fb);
_vel_timestamp_ms_buffers[id].put(time_now_ms);
}
loop_profiler.stop("lock");
loop_profiler.start();

Expand Down

0 comments on commit 3ce8cb5

Please sign in to comment.