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

driver and hand tested successful.. next is coppeliamirror node

Browse Source
This commit is contained in:
qlin960618
2024-08-15 20:19:28 +09:00
parent 058c123170
commit 94a32a0f0c
69 changed files with 89 additions and 12379 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/joint/robot_interface_franka.cpp
View File

@@ -311,7 +311,7 @@ void RobotInterfaceFranka::initialize()
// initialize and set the robot to default behavior (collision behavior, impedance, etc)
// robot_sptr_->setDefaultBehavior();
// setDefaultBehavior(*robot_sptr_);
setDefaultBehavior(*robot_sptr_);
robot_sptr_->setCollisionBehavior(
franka_configuration_.lower_torque_threshold,
franka_configuration_.upper_torque_threshold,

11
src/joint/sas_robot_driver_franka.cpp
View File

@@ -40,12 +40,12 @@ namespace sas
{
RobotDriverFranka::RobotDriverFranka(
const std::shared_ptr<Node> &node,
qros::RobotDynamicsServer* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
):
node_(node),
RobotDriver(break_loops),
configuration_(configuration),
robot_dynamic_provider_(robot_dynamic_provider),
robot_dynamic_provider_sptr_(robot_dynamic_provider),
break_loops_(break_loops)
{
joint_positions_.resize(7);
@@ -93,7 +93,7 @@ namespace sas
Vector3d force, torque;
std::tie(force, torque) = robot_driver_interface_sptr_->get_stiffness_force_torque();
const auto pose = robot_driver_interface_sptr_->get_stiffness_pose();
robot_dynamic_provider_->publish_stiffness(pose, force, torque);
robot_dynamic_provider_sptr_->publish_stiffness(pose, force, torque);
}
@@ -151,6 +151,11 @@ namespace sas
"error on:"+robot_driver_interface_sptr_->get_status_message());
break_loops_->store(true);
}
if(!ok()) {
RCLCPP_WARN_STREAM(node_->get_logger(),
"["+std::string(node_->get_name())+"]::driver interface exit on shotdown signal recieved."
);
}
_update_stiffness_contact_and_pose();
return robot_driver_interface_sptr_->get_joint_positions();
}

20
src/robot_dynamics/qros_robot_dynamics_server.cpp
View File

@@ -34,7 +34,9 @@ using namespace qros;
RobotDynamicsServer::RobotDynamicsServer(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
world_to_base_tf_(0)
world_to_base_tf_(0),
tf_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
static_base_tf_broadcaster_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_))
{
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
@@ -74,10 +76,12 @@ void RobotDynamicsServer::set_world_to_base_tf(const DQ& world_to_base_tf)
void RobotDynamicsServer::_publish_base_static_tf()
{
geometry_msgs::msg::TransformStamped base_tf;
base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
base_tf.header.stamp = node_->now();
base_tf.header.frame_id = WORLD_FRAME_ID;
base_tf.child_frame_id = parent_frame_id_;
base_tf.set__transform(_dq_to_geometry_msgs_transform(world_to_base_tf_));
std_msgs::msg::Header header;
header.set__stamp(node_->now());
header.set__frame_id(WORLD_FRAME_ID);
base_tf.set__header(header);
base_tf.set__child_frame_id(parent_frame_id_);
static_base_tf_broadcaster_->sendTransform(base_tf);
}
@@ -88,7 +92,7 @@ void RobotDynamicsServer::publish_stiffness(const DQ& base_to_stiffness, const V
std_msgs::msg::Header header;
header.set__stamp(node_->now());
geometry_msgs::msg::WrenchStamped msg;
msg.header = header;
msg.set__header(header);
msg.header.set__frame_id(child_frame_id_);
msg.wrench.force.set__x(force(0));
msg.wrench.force.set__y(force(1));
@@ -99,10 +103,10 @@ void RobotDynamicsServer::publish_stiffness(const DQ& base_to_stiffness, const V
publisher_cartesian_stiffness_->publish(msg);
if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
{
header.set__frame_id(parent_frame_id_);
geometry_msgs::msg::TransformStamped tf_msg;
tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
tf_msg.set__transform(_dq_to_geometry_msgs_transform(base_to_stiffness));
tf_msg.set__header(header);
tf_msg.header.set__frame_id(parent_frame_id_);
tf_msg.set__child_frame_id(child_frame_id_);
tf_broadcaster_->sendTransform(tf_msg);
}

18
src/sas_robot_driver_franka_node.cpp
View File

@@ -101,11 +101,15 @@ int main(int argc, char **argv)
sas::get_ros_parameter(node,"robot_mode", robot_driver_franka_configuration.mode);
double upper_scale_factor = 1.0;
std::vector<std::string> all_params;
if(node->has_parameter("force_upper_limits_scaling_factor"))
{
try {
sas::get_ros_parameter(node,"force_upper_limits_scaling_factor",upper_scale_factor);
RCLCPP_WARN_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Set force upper limits scaling factor: " << upper_scale_factor);
RCLCPP_WARN_STREAM_ONCE(node->get_logger(),"Set force upper limits scaling factor: " << upper_scale_factor);
}catch(...) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
}
try{node->declare_parameter<std::vector<double>>("upper_torque_threshold");}catch (...){}
if(node->has_parameter("upper_torque_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
std::vector<double> upper_torque_threshold_std_vec;
@@ -115,6 +119,7 @@ int main(int argc, char **argv)
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
}
try{node->declare_parameter<std::vector<double>>("upper_force_threshold");}catch (...){}
if(node->has_parameter("upper_force_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
std::vector<double> upper_torque_threshold_std_vec;
@@ -124,6 +129,7 @@ int main(int argc, char **argv)
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
}
try{node->declare_parameter<std::string>("robot_parameter_file_path");}catch (...){}
if(node->has_parameter("robot_parameter_file_path"))
{
std::string robot_parameter_file_path;
@@ -143,10 +149,10 @@ int main(int argc, char **argv)
// initialize the robot dynamic provider
robot_driver_ros_configuration.robot_driver_provider_prefix = node_name;
qros::RobotDynamicsServer robot_dynamic_provider(node, robot_driver_ros_configuration.robot_driver_provider_prefix);
std::shared_ptr<qros::RobotDynamicsServer> robot_dynamic_provider_ptr = std::make_shared<qros::RobotDynamicsServer>(node, robot_driver_ros_configuration.robot_driver_provider_prefix);
if(robot_driver_franka_configuration.robot_reference_frame!=0)
{
robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
robot_dynamic_provider_ptr->set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
}
try
@@ -154,7 +160,7 @@ int main(int argc, char **argv)
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating Franka robot.");
auto robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
node,
&robot_dynamic_provider,
robot_dynamic_provider_ptr,
robot_driver_franka_configuration,
&kill_this_process
);