qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added cartesian interface

Browse Source
This commit is contained in:
qlin1806
2024-07-19 18:35:17 -07:00
parent a68afa690d
commit 9d65b88cb3
8 changed files with 178 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/sas_robot_driver_franka.cpp
View File

@@ -39,9 +39,10 @@
namespace sas
{
RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
RobotDriverFranka::RobotDriverFranka(RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
RobotDriver(break_loops),
configuration_(configuration),
robot_dynamic_provider_(robot_dynamic_provider),
break_loops_(break_loops)
{
joint_positions_.resize(7);
@@ -81,6 +82,17 @@ namespace sas
RobotDriverFranka::~RobotDriverFranka()=default;
void RobotDriverFranka::_update_cartesian_contact_and_pose() const
{
Vector3d force, torque;
std::tie(force, torque) = robot_driver_interface_sptr_->get_cartesian_force_torque();
const auto pose = robot_driver_interface_sptr_->get_cartesian_pose();
robot_dynamic_provider_->publish_cartesian_contact(force, torque);
robot_dynamic_provider_->publish_cartesian_pose(pose);
}
/**
* @brief
*
@@ -132,6 +144,7 @@ namespace sas
ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
break_loops_->store(true);
}
_update_cartesian_contact_and_pose();
return robot_driver_interface_sptr_->get_joint_positions();
}