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

more optimization..

Browse Source
This commit is contained in:
qlin1806
2024-07-21 03:52:21 -07:00
parent 46668deac2
commit fce21f08b9
9 changed files with 178 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
CMakeLists.txt
View File

@@ -16,24 +16,21 @@ add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTR
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
tf2_ros
tf2
sas_common
sas_clock
sas_robot_driver
sas_patient_side_manager
roscpp
rospy
std_msgs
tf2_ros
tf2
sas_common
sas_clock
sas_robot_driver
sas_patient_side_manager
pybind11_catkin
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2
pybind11_catkin
INCLUDE_DIRS include
CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin
)
find_package(Franka REQUIRED)
@@ -46,9 +43,9 @@ set(CMAKE_PREFIX_PATH $ENV{QT_PATH})
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTORCC ON)
set(CMAKE_AUTOUIC ON)
if(CMAKE_VERSION VERSION_LESS "3.7.0")
if (CMAKE_VERSION VERSION_LESS "3.7.0")
set(CMAKE_INCLUDE_CURRENT_DIR ON)
endif()
endif ()
find_package(Qt5 COMPONENTS Widgets REQUIRED)
find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets)
@@ -63,27 +60,27 @@ add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp)
add_library(QuadraticProgramMotionGenerator src/generator/quadratic_program_motion_generator.cpp)
target_link_libraries(QuadraticProgramMotionGenerator
qpOASES
dqrobotics
ConstraintsManager)
qpOASES
dqrobotics
ConstraintsManager)
add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp)
target_link_libraries(CustomMotionGeneration
qpOASES
dqrobotics
ConstraintsManager)
qpOASES
dqrobotics
ConstraintsManager)
add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
target_link_libraries(robot_interface_franka Franka::Franka
dqrobotics
MotionGenerator
ConstraintsManager
QuadraticProgramMotionGenerator
CustomMotionGeneration)
dqrobotics
MotionGenerator
ConstraintsManager
QuadraticProgramMotionGenerator
CustomMotionGeneration)
add_library(robot_interface_hand src/hand/robot_interface_hand.cpp)
target_link_libraries(robot_interface_hand Franka::Franka
dqrobotics)
dqrobotics)
############
@@ -95,15 +92,15 @@ target_link_libraries(robot_interface_hand Franka::Franka
include_directories(
include
include/generator
src/
include
include/generator
src/
src/robot_dynamics
src/hand
src/joint
${catkin_INCLUDE_DIRS}
constraints_manager/include
)
src/hand
src/joint
${catkin_INCLUDE_DIRS}
constraints_manager/include
)
add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp)
@@ -119,9 +116,10 @@ target_link_libraries(qros_robot_dynamics_interface
add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
target_link_libraries(sas_robot_driver_franka
qros_robot_dynamics_provider
dqrobotics
robot_interface_franka)
qros_robot_dynamics_provider
dqrobotics
dqrobotics-interface-json11
robot_interface_franka)
add_library(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp)
target_link_libraries(sas_robot_driver_franka_hand
@@ -131,21 +129,21 @@ target_link_libraries(sas_robot_driver_franka_hand
add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
target_link_libraries(sas_robot_driver_coppelia
dqrobotics
dqrobotics-interface-vrep)
dqrobotics
dqrobotics-interface-vrep)
add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp)
target_link_libraries(sas_robot_driver_coppelia_node
sas_robot_driver_coppelia
${catkin_LIBRARIES})
sas_robot_driver_coppelia
${catkin_LIBRARIES})
add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp)
target_link_libraries(sas_robot_driver_franka_node
sas_robot_driver_franka
${catkin_LIBRARIES})
sas_robot_driver_franka
${catkin_LIBRARIES})
add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
@@ -154,19 +152,17 @@ target_link_libraries(sas_robot_driver_franka_hand_node
${catkin_LIBRARIES})
add_executable(JuankaEmika
qt/configuration_window/main.cpp
qt/configuration_window/mainwindow.cpp
qt/configuration_window/mainwindow.ui
)
target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
dqrobotics
${catkin_LIBRARIES}
robot_interface_franka
)
qt/configuration_window/main.cpp
qt/configuration_window/mainwindow.cpp
qt/configuration_window/mainwindow.ui
)
target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
dqrobotics
${catkin_LIBRARIES}
robot_interface_franka
)
#####################################################################################
@@ -182,33 +178,31 @@ target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND)
target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics)
if(QT_VERSION_MAJOR EQUAL 6)
if (QT_VERSION_MAJOR EQUAL 6)
qt_finalize_executable(JuankaEmika)
endif()
endif ()
install(TARGETS ${PROJECT_NAME}
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(TARGETS sas_robot_driver_franka_node
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS sas_robot_driver_coppelia_node
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS sas_robot_driver_franka_hand_node
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
)

9
include/robot_dynamic/qros_robot_dynamics_provider.h
View File

@@ -36,12 +36,14 @@
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros//static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Header.h>
#include <dqrobotics/DQ.h>
#define REDUCE_TF_PUBLISH_RATE 10
#define WORLD_FRAME_ID "world"
namespace qros {
@@ -57,9 +59,13 @@ private:
ros::Publisher publisher_cartesian_stiffness_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
tf2_ros::StaticTransformBroadcaster static_base_tf_broadcaster_;
DQ world_to_base_tf_;
static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ;
static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose);
void _publish_base_static_tf();
public:
RobotDynamicProvider() = delete;
@@ -72,6 +78,7 @@ public:
void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque);
void set_world_to_base_tf(const DQ& world_to_base_tf);
bool is_enabled() const;
std::string get_topic_prefix() const {return node_prefix_;}

1
include/sas_robot_driver_franka.h
View File

@@ -60,6 +60,7 @@ struct RobotDriverFrankaConfiguration
int port;
double speed;
RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
DQ robot_reference_frame = DQ(0);
};

29
scripts/publish_dumy_robot_dynmaics.py Normal file
View File

@@ -0,0 +1,29 @@
import rospy
from sas_robot_driver_franka import RobotDynamicsProvider
import dqrobotics as dql
import numpy as np
if __name__ == "__main__":
rospy.init_node("dummy_robot_dynamics_provider")
dynam_provider = RobotDynamicsProvider("/franka1")
t = dql.DQ([0, 0, 1])
r = dql.DQ([1, 0, 0, 0])
base_pose = r + 0.5 * dql.E_ * t * r
dynam_provider.set_world_to_base_tf(base_pose)
t_ = 0
rospy.loginfo("Publishing dummy robot dynamics")
r = dql.DQ([0, 0, 0, 1])
rate = rospy.Rate(100)
dummy_force = np.random.rand(3) * 100
dummy_torque = np.random.rand(3) * 10
while not rospy.is_shutdown():
t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
ee_pose = r + 0.5 * dql.E_ * t * r
dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
rate.sleep()
t_ += 0.01

30
scripts/subscribe_dummy_dynamic.py Normal file
View File

@@ -0,0 +1,30 @@
import rospy
from sas_robot_driver_franka import RobotDynamicsInterface
import dqrobotics as dql
import numpy as np
from dqrobotics.interfaces.vrep import DQ_VrepInterface
vrep = DQ_VrepInterface()
vrep.connect("192.168.10.103", 19997, 100, 10)
if __name__ == "__main__":
rospy.init_node("dummy_robot_dynamics_subscriber")
dynam_provider = RobotDynamicsInterface("/franka1")
while not dynam_provider.is_enabled():
rospy.loginfo("Waiting for robot dynamics provider to be enabled")
rospy.sleep(1)
rospy.loginfo("Subscribing to dummy robot dynamics")
rate = rospy.Rate(50)
while not rospy.is_shutdown():
force = dynam_provider.get_stiffness_force()
torque = dynam_provider.get_stiffness_torque()
ee_pose = dynam_provider.get_stiffness_frame_pose()
vrep.set_object_pose("xd1", ee_pose)
rospy.loginfo("EE Pose: %s", ee_pose)
rospy.loginfo("Force: %s", force)
rospy.loginfo("Torque: %s", torque)
rate.sleep()

1
src/robot_dynamics/qros_robot_dynamic_py.cpp
View File

@@ -53,6 +53,7 @@ PYBIND11_MODULE(_qros_robot_dynamic, m)
py::class_<RDP>(m, "RobotDynamicsProvider")
.def(py::init<const std::string&>())
.def("publish_stiffness",&RDP::publish_stiffness)
.def("set_world_to_base_tf", &RDP::set_world_to_base_tf)
.def("is_enabled",&RDP::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
.def("get_topic_prefix",&RDP::get_topic_prefix);

6
src/robot_dynamics/qros_robot_dynamics_interface.cpp
View File

@@ -47,8 +47,10 @@ RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &publisher_nodehand
last_stiffness_torque_(Vector3d::Zero()),
last_stiffness_frame_pose_(0)
{
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicInterface with prefix " + node_prefix);
subscriber_cartesian_stiffness_ = subscriber_nodehandle.subscribe(node_prefix_ + "/get/cartesian_stiffness", 1, &RobotDynamicInterface::_callback_cartesian_stiffness, this);
}
@@ -64,7 +66,7 @@ void RobotDynamicInterface::_callback_cartesian_stiffness(const geometry_msgs::W
last_stiffness_torque_(2) = msg->wrench.torque.z;
try {
const auto transform_stamped = tf_buffer_.lookupTransform(parent_frame_id_, child_frame_id_, ros::Time(0));
const auto transform_stamped = tf_buffer_.lookupTransform( parent_frame_id_, child_frame_id_, ros::Time(0));
last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
} catch (tf2::TransformException &ex) {
ROS_WARN_STREAM(ros::this_node::getName() + "::" + ex.what());

29
src/robot_dynamics/qros_robot_dynamics_provider.cpp
View File

@@ -40,8 +40,13 @@ RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const st
RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
node_prefix_(node_prefix),
child_frame_id_(node_prefix + "_stiffness_frame"),
parent_frame_id_(node_prefix + "_base")
parent_frame_id_(node_prefix + "_base"),
world_to_base_tf_(0)
{
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix);
publisher_cartesian_stiffness_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_stiffness", 1);
@@ -63,6 +68,28 @@ geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(co
return tf_msg;
}
void RobotDynamicProvider::set_world_to_base_tf(const DQ& world_to_base_tf)
{
if(world_to_base_tf_==0)
{
world_to_base_tf_ = world_to_base_tf;
_publish_base_static_tf();
}else
{
throw std::runtime_error("The world to base transform has already been set");
}
}
void RobotDynamicProvider::_publish_base_static_tf()
{
geometry_msgs::TransformStamped base_tf;
base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
base_tf.header.stamp = ros::Time::now();
base_tf.header.frame_id = WORLD_FRAME_ID;
base_tf.child_frame_id = parent_frame_id_;
static_base_tf_broadcaster_.sendTransform(base_tf);
}
void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)

16
src/sas_robot_driver_franka_node.cpp
View File

@@ -35,7 +35,7 @@
#include <exception>
#include <dqrobotics/utils/DQ_Math.h>
//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
#include <sas_common/sas_common.h>
#include <sas_conversions/eigen3_std_conversions.h>
#include <sas_robot_driver/sas_robot_driver_ros.h>
@@ -121,7 +121,14 @@ int main(int argc, char **argv)
ROS_INFO_STREAM(ros::this_node::getName()+"::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);
}
if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path"))
{
std::string robot_parameter_file_path;
sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path);
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading robot parameters from file: " + robot_parameter_file_path);
const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
}else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}
robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
@@ -134,7 +141,10 @@ int main(int argc, char **argv)
// initialize the robot dynamic provider
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
qros::RobotDynamicProvider robot_dynamic_provider(nh, 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);
}
try
{