diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index 7e2c4752..ca8c3eba 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -54,11 +54,13 @@ foreach(sub_dir ${srv_generated_dir_list}) add_service_files(DIRECTORY srv/generated/${sub_dir}) endforeach() +add_action_files(DIRECTORY action/non_generated) + ## generate added messages and services -generate_messages(DEPENDENCIES std_msgs) +generate_messages(DEPENDENCIES std_msgs actionlib_msgs) ## declare a catkin package -catkin_package() +catkin_package(CATKIN_DEPENDS actionlib_msgs) if(USE_CONAN) # Include conan.cmake module and download Kortex API from artifactory diff --git a/kortex_driver/action/non_generated/FollowCartesianTrajectory.action b/kortex_driver/action/non_generated/FollowCartesianTrajectory.action new file mode 100644 index 00000000..9e1e3171 --- /dev/null +++ b/kortex_driver/action/non_generated/FollowCartesianTrajectory.action @@ -0,0 +1,15 @@ +#The trajectory to follow +CartesianWaypoint[] trajectory +duration goal_time_tolerance +bool use_optimal_blending +--- +#result definition +string error_string +int32 error_code +int32 SUCCESSFUL = 0 +int32 INVALID_GOAL = -1 +int32 PATH_TOLERANCE_VIOLATED = -2 + +--- +#feedback +CartesianWaypoint actual \ No newline at end of file diff --git a/kortex_driver/conanfile.py b/kortex_driver/conanfile.py index d494effe..e30328ff 100644 --- a/kortex_driver/conanfile.py +++ b/kortex_driver/conanfile.py @@ -6,4 +6,4 @@ class ROSKortexConan(ConanFile): def requirements(self): - self.requires("kortex_api_cpp/2.2.0-r.31@kortex/stable") + self.requires("kortex_api_cpp/2.3.0-r.34@kortex/stable") diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h index 1df8686a..e26e9a8b 100644 --- a/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h +++ b/kortex_driver/include/kortex_driver/generated/interfaces/base_services_interface.h @@ -84,8 +84,8 @@ #include "kortex_driver/ConfigurationChangeNotification.h" #include "kortex_driver/OnNotificationMappingInfoTopic.h" #include "kortex_driver/MappingInfoNotification.h" -#include "kortex_driver/OnNotificationControlModeTopic.h" -#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/Base_OnNotificationControlModeTopic.h" +#include "kortex_driver/Base_ControlModeNotification.h" #include "kortex_driver/OnNotificationOperatingModeTopic.h" #include "kortex_driver/OperatingModeNotification.h" #include "kortex_driver/OnNotificationSequenceInfoTopic.h" @@ -176,12 +176,16 @@ #include "kortex_driver/DeleteAllSequenceTasks.h" #include "kortex_driver/TakeSnapshot.h" #include "kortex_driver/GetFirmwareBundleVersions.h" +#include "kortex_driver/ExecuteWaypointTrajectory.h" #include "kortex_driver/MoveSequenceTask.h" #include "kortex_driver/DuplicateMapping.h" #include "kortex_driver/DuplicateMap.h" #include "kortex_driver/SetControllerConfiguration.h" #include "kortex_driver/GetControllerConfiguration.h" #include "kortex_driver/GetAllControllerConfigurations.h" +#include "kortex_driver/ComputeForwardKinematics.h" +#include "kortex_driver/ComputeInverseKinematics.h" +#include "kortex_driver/ValidateWaypointList.h" #include "kortex_driver/KortexError.h" #include "kortex_driver/SetDeviceID.h" @@ -257,7 +261,7 @@ class IBaseServices virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) = 0; virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) = 0; virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) = 0; - virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) = 0; + virtual bool Base_OnNotificationControlModeTopic(kortex_driver::Base_OnNotificationControlModeTopic::Request &req, kortex_driver::Base_OnNotificationControlModeTopic::Response &res) = 0; virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) = 0; virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) = 0; virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) = 0; @@ -349,12 +353,16 @@ class IBaseServices virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) = 0; virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) = 0; virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) = 0; + virtual bool ExecuteWaypointTrajectory(kortex_driver::ExecuteWaypointTrajectory::Request &req, kortex_driver::ExecuteWaypointTrajectory::Response &res) = 0; virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) = 0; virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) = 0; virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) = 0; virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) = 0; virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) = 0; virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) = 0; + virtual bool ComputeForwardKinematics(kortex_driver::ComputeForwardKinematics::Request &req, kortex_driver::ComputeForwardKinematics::Response &res) = 0; + virtual bool ComputeInverseKinematics(kortex_driver::ComputeInverseKinematics::Request &req, kortex_driver::ComputeInverseKinematics::Response &res) = 0; + virtual bool ValidateWaypointList(kortex_driver::ValidateWaypointList::Request &req, kortex_driver::ValidateWaypointList::Response &res) = 0; protected: ros::NodeHandle m_node_handle; @@ -449,7 +457,7 @@ class IBaseServices ros::ServiceServer m_serviceBase_Unsubscribe; ros::ServiceServer m_serviceOnNotificationConfigurationChangeTopic; ros::ServiceServer m_serviceOnNotificationMappingInfoTopic; - ros::ServiceServer m_serviceOnNotificationControlModeTopic; + ros::ServiceServer m_serviceBase_OnNotificationControlModeTopic; ros::ServiceServer m_serviceOnNotificationOperatingModeTopic; ros::ServiceServer m_serviceOnNotificationSequenceInfoTopic; ros::ServiceServer m_serviceOnNotificationProtectionZoneTopic; @@ -529,11 +537,15 @@ class IBaseServices ros::ServiceServer m_serviceDeleteAllSequenceTasks; ros::ServiceServer m_serviceTakeSnapshot; ros::ServiceServer m_serviceGetFirmwareBundleVersions; + ros::ServiceServer m_serviceExecuteWaypointTrajectory; ros::ServiceServer m_serviceMoveSequenceTask; ros::ServiceServer m_serviceDuplicateMapping; ros::ServiceServer m_serviceDuplicateMap; ros::ServiceServer m_serviceSetControllerConfiguration; ros::ServiceServer m_serviceGetControllerConfiguration; ros::ServiceServer m_serviceGetAllControllerConfigurations; + ros::ServiceServer m_serviceComputeForwardKinematics; + ros::ServiceServer m_serviceComputeInverseKinematics; + ros::ServiceServer m_serviceValidateWaypointList; }; #endif diff --git a/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h b/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h index 6475c3db..c5a4b8a7 100644 --- a/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h +++ b/kortex_driver/include/kortex_driver/generated/interfaces/controlconfig_services_interface.h @@ -54,6 +54,8 @@ #include "kortex_driver/ResetTwistLinearSoftLimit.h" #include "kortex_driver/ResetTwistAngularSoftLimit.h" #include "kortex_driver/ResetJointAccelerationSoftLimits.h" +#include "kortex_driver/ControlConfig_OnNotificationControlModeTopic.h" +#include "kortex_driver/ControlConfig_ControlModeNotification.h" #include "kortex_driver/KortexError.h" #include "kortex_driver/SetDeviceID.h" @@ -99,12 +101,16 @@ class IControlConfigServices virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) = 0; virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) = 0; virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) = 0; + virtual bool ControlConfig_OnNotificationControlModeTopic(kortex_driver::ControlConfig_OnNotificationControlModeTopic::Request &req, kortex_driver::ControlConfig_OnNotificationControlModeTopic::Response &res) = 0; + virtual void cb_ControlModeTopic(Kinova::Api::ControlConfig::ControlModeNotification notif) = 0; protected: ros::NodeHandle m_node_handle; ros::Publisher m_pub_Error; ros::Publisher m_pub_ControlConfigurationTopic; bool m_is_activated_ControlConfigurationTopic; + ros::Publisher m_pub_ControlModeTopic; + bool m_is_activated_ControlModeTopic; ros::ServiceServer m_serviceSetDeviceID; ros::ServiceServer m_serviceSetApiOptions; @@ -138,5 +144,6 @@ class IControlConfigServices ros::ServiceServer m_serviceResetTwistLinearSoftLimit; ros::ServiceServer m_serviceResetTwistAngularSoftLimit; ros::ServiceServer m_serviceResetJointAccelerationSoftLimits; + ros::ServiceServer m_serviceControlConfig_OnNotificationControlModeTopic; }; #endif diff --git a/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h index 23cc90cf..c8b68fee 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_proto_converter.h @@ -41,6 +41,9 @@ #include "kortex_driver/generated/robot/visionconfig_proto_converter.h" +#include "kortex_driver/GpioConfigurationList.h" +#include "kortex_driver/Base_GpioConfiguration.h" +#include "kortex_driver/GpioPinConfiguration.h" #include "kortex_driver/FullUserProfile.h" #include "kortex_driver/UserProfile.h" #include "kortex_driver/UserProfileList.h" @@ -99,7 +102,7 @@ #include "kortex_driver/ConfigurationChangeNotification.h" #include "kortex_driver/MappingInfoNotification.h" #include "kortex_driver/Base_ControlModeInformation.h" -#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/Base_ControlModeNotification.h" #include "kortex_driver/ServoingModeInformation.h" #include "kortex_driver/OperatingModeInformation.h" #include "kortex_driver/OperatingModeNotification.h" @@ -116,6 +119,7 @@ #include "kortex_driver/ControllerState.h" #include "kortex_driver/ControllerElementState.h" #include "kortex_driver/ActionNotification.h" +#include "kortex_driver/TrajectoryInfo.h" #include "kortex_driver/ActionExecutionState.h" #include "kortex_driver/RobotEventNotification.h" #include "kortex_driver/FactoryNotification.h" @@ -176,6 +180,7 @@ #include "kortex_driver/GripperRequest.h" #include "kortex_driver/Gripper.h" #include "kortex_driver/Finger.h" +#include "kortex_driver/GpioCommand.h" #include "kortex_driver/SystemTime.h" #include "kortex_driver/ControllerConfigurationMode.h" #include "kortex_driver/ControllerConfiguration.h" @@ -193,10 +198,20 @@ #include "kortex_driver/PreComputedJointTrajectoryElement.h" #include "kortex_driver/TrajectoryErrorElement.h" #include "kortex_driver/TrajectoryErrorReport.h" +#include "kortex_driver/WaypointValidationReport.h" +#include "kortex_driver/Waypoint.h" +#include "kortex_driver/AngularWaypoint.h" +#include "kortex_driver/CartesianWaypoint.h" +#include "kortex_driver/WaypointList.h" +#include "kortex_driver/KinematicTrajectoryConstraints.h" #include "kortex_driver/FirmwareBundleVersions.h" #include "kortex_driver/FirmwareComponentVersion.h" +#include "kortex_driver/IKData.h" +int ToProtoData(kortex_driver::GpioConfigurationList input, Kinova::Api::Base::GpioConfigurationList *output); +int ToProtoData(kortex_driver::Base_GpioConfiguration input, Kinova::Api::Base::GpioConfiguration *output); +int ToProtoData(kortex_driver::GpioPinConfiguration input, Kinova::Api::Base::GpioPinConfiguration *output); int ToProtoData(kortex_driver::FullUserProfile input, Kinova::Api::Base::FullUserProfile *output); int ToProtoData(kortex_driver::UserProfile input, Kinova::Api::Base::UserProfile *output); int ToProtoData(kortex_driver::UserProfileList input, Kinova::Api::Base::UserProfileList *output); @@ -255,7 +270,7 @@ int ToProtoData(kortex_driver::Query input, Kinova::Api::Base::Query *output); int ToProtoData(kortex_driver::ConfigurationChangeNotification input, Kinova::Api::Base::ConfigurationChangeNotification *output); int ToProtoData(kortex_driver::MappingInfoNotification input, Kinova::Api::Base::MappingInfoNotification *output); int ToProtoData(kortex_driver::Base_ControlModeInformation input, Kinova::Api::Base::ControlModeInformation *output); -int ToProtoData(kortex_driver::ControlModeNotification input, Kinova::Api::Base::ControlModeNotification *output); +int ToProtoData(kortex_driver::Base_ControlModeNotification input, Kinova::Api::Base::ControlModeNotification *output); int ToProtoData(kortex_driver::ServoingModeInformation input, Kinova::Api::Base::ServoingModeInformation *output); int ToProtoData(kortex_driver::OperatingModeInformation input, Kinova::Api::Base::OperatingModeInformation *output); int ToProtoData(kortex_driver::OperatingModeNotification input, Kinova::Api::Base::OperatingModeNotification *output); @@ -272,6 +287,7 @@ int ToProtoData(kortex_driver::ControllerList input, Kinova::Api::Base::Controll int ToProtoData(kortex_driver::ControllerState input, Kinova::Api::Base::ControllerState *output); int ToProtoData(kortex_driver::ControllerElementState input, Kinova::Api::Base::ControllerElementState *output); int ToProtoData(kortex_driver::ActionNotification input, Kinova::Api::Base::ActionNotification *output); +int ToProtoData(kortex_driver::TrajectoryInfo input, Kinova::Api::Base::TrajectoryInfo *output); int ToProtoData(kortex_driver::ActionExecutionState input, Kinova::Api::Base::ActionExecutionState *output); int ToProtoData(kortex_driver::RobotEventNotification input, Kinova::Api::Base::RobotEventNotification *output); int ToProtoData(kortex_driver::FactoryNotification input, Kinova::Api::Base::FactoryNotification *output); @@ -332,6 +348,7 @@ int ToProtoData(kortex_driver::GripperCommand input, Kinova::Api::Base::GripperC int ToProtoData(kortex_driver::GripperRequest input, Kinova::Api::Base::GripperRequest *output); int ToProtoData(kortex_driver::Gripper input, Kinova::Api::Base::Gripper *output); int ToProtoData(kortex_driver::Finger input, Kinova::Api::Base::Finger *output); +int ToProtoData(kortex_driver::GpioCommand input, Kinova::Api::Base::GpioCommand *output); int ToProtoData(kortex_driver::SystemTime input, Kinova::Api::Base::SystemTime *output); int ToProtoData(kortex_driver::ControllerConfigurationMode input, Kinova::Api::Base::ControllerConfigurationMode *output); int ToProtoData(kortex_driver::ControllerConfiguration input, Kinova::Api::Base::ControllerConfiguration *output); @@ -349,7 +366,14 @@ int ToProtoData(kortex_driver::PreComputedJointTrajectory input, Kinova::Api::Ba int ToProtoData(kortex_driver::PreComputedJointTrajectoryElement input, Kinova::Api::Base::PreComputedJointTrajectoryElement *output); int ToProtoData(kortex_driver::TrajectoryErrorElement input, Kinova::Api::Base::TrajectoryErrorElement *output); int ToProtoData(kortex_driver::TrajectoryErrorReport input, Kinova::Api::Base::TrajectoryErrorReport *output); +int ToProtoData(kortex_driver::WaypointValidationReport input, Kinova::Api::Base::WaypointValidationReport *output); +int ToProtoData(kortex_driver::Waypoint input, Kinova::Api::Base::Waypoint *output); +int ToProtoData(kortex_driver::AngularWaypoint input, Kinova::Api::Base::AngularWaypoint *output); +int ToProtoData(kortex_driver::CartesianWaypoint input, Kinova::Api::Base::CartesianWaypoint *output); +int ToProtoData(kortex_driver::WaypointList input, Kinova::Api::Base::WaypointList *output); +int ToProtoData(kortex_driver::KinematicTrajectoryConstraints input, Kinova::Api::Base::KinematicTrajectoryConstraints *output); int ToProtoData(kortex_driver::FirmwareBundleVersions input, Kinova::Api::Base::FirmwareBundleVersions *output); int ToProtoData(kortex_driver::FirmwareComponentVersion input, Kinova::Api::Base::FirmwareComponentVersion *output); +int ToProtoData(kortex_driver::IKData input, Kinova::Api::Base::IKData *output); #endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h index 0cb57ed1..f1e2b4a8 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_ros_converter.h @@ -41,6 +41,9 @@ #include "kortex_driver/generated/robot/visionconfig_ros_converter.h" +#include "kortex_driver/GpioConfigurationList.h" +#include "kortex_driver/Base_GpioConfiguration.h" +#include "kortex_driver/GpioPinConfiguration.h" #include "kortex_driver/FullUserProfile.h" #include "kortex_driver/UserProfile.h" #include "kortex_driver/UserProfileList.h" @@ -99,7 +102,7 @@ #include "kortex_driver/ConfigurationChangeNotification.h" #include "kortex_driver/MappingInfoNotification.h" #include "kortex_driver/Base_ControlModeInformation.h" -#include "kortex_driver/ControlModeNotification.h" +#include "kortex_driver/Base_ControlModeNotification.h" #include "kortex_driver/ServoingModeInformation.h" #include "kortex_driver/OperatingModeInformation.h" #include "kortex_driver/OperatingModeNotification.h" @@ -116,6 +119,7 @@ #include "kortex_driver/ControllerState.h" #include "kortex_driver/ControllerElementState.h" #include "kortex_driver/ActionNotification.h" +#include "kortex_driver/TrajectoryInfo.h" #include "kortex_driver/ActionExecutionState.h" #include "kortex_driver/RobotEventNotification.h" #include "kortex_driver/FactoryNotification.h" @@ -176,6 +180,7 @@ #include "kortex_driver/GripperRequest.h" #include "kortex_driver/Gripper.h" #include "kortex_driver/Finger.h" +#include "kortex_driver/GpioCommand.h" #include "kortex_driver/SystemTime.h" #include "kortex_driver/ControllerConfigurationMode.h" #include "kortex_driver/ControllerConfiguration.h" @@ -193,10 +198,20 @@ #include "kortex_driver/PreComputedJointTrajectoryElement.h" #include "kortex_driver/TrajectoryErrorElement.h" #include "kortex_driver/TrajectoryErrorReport.h" +#include "kortex_driver/WaypointValidationReport.h" +#include "kortex_driver/Waypoint.h" +#include "kortex_driver/AngularWaypoint.h" +#include "kortex_driver/CartesianWaypoint.h" +#include "kortex_driver/WaypointList.h" +#include "kortex_driver/KinematicTrajectoryConstraints.h" #include "kortex_driver/FirmwareBundleVersions.h" #include "kortex_driver/FirmwareComponentVersion.h" +#include "kortex_driver/IKData.h" +int ToRosData(Kinova::Api::Base::GpioConfigurationList input, kortex_driver::GpioConfigurationList &output); +int ToRosData(Kinova::Api::Base::GpioConfiguration input, kortex_driver::Base_GpioConfiguration &output); +int ToRosData(Kinova::Api::Base::GpioPinConfiguration input, kortex_driver::GpioPinConfiguration &output); int ToRosData(Kinova::Api::Base::FullUserProfile input, kortex_driver::FullUserProfile &output); int ToRosData(Kinova::Api::Base::UserProfile input, kortex_driver::UserProfile &output); int ToRosData(Kinova::Api::Base::UserProfileList input, kortex_driver::UserProfileList &output); @@ -255,7 +270,7 @@ int ToRosData(Kinova::Api::Base::Query input, kortex_driver::Query &output); int ToRosData(Kinova::Api::Base::ConfigurationChangeNotification input, kortex_driver::ConfigurationChangeNotification &output); int ToRosData(Kinova::Api::Base::MappingInfoNotification input, kortex_driver::MappingInfoNotification &output); int ToRosData(Kinova::Api::Base::ControlModeInformation input, kortex_driver::Base_ControlModeInformation &output); -int ToRosData(Kinova::Api::Base::ControlModeNotification input, kortex_driver::ControlModeNotification &output); +int ToRosData(Kinova::Api::Base::ControlModeNotification input, kortex_driver::Base_ControlModeNotification &output); int ToRosData(Kinova::Api::Base::ServoingModeInformation input, kortex_driver::ServoingModeInformation &output); int ToRosData(Kinova::Api::Base::OperatingModeInformation input, kortex_driver::OperatingModeInformation &output); int ToRosData(Kinova::Api::Base::OperatingModeNotification input, kortex_driver::OperatingModeNotification &output); @@ -272,6 +287,7 @@ int ToRosData(Kinova::Api::Base::ControllerList input, kortex_driver::Controller int ToRosData(Kinova::Api::Base::ControllerState input, kortex_driver::ControllerState &output); int ToRosData(Kinova::Api::Base::ControllerElementState input, kortex_driver::ControllerElementState &output); int ToRosData(Kinova::Api::Base::ActionNotification input, kortex_driver::ActionNotification &output); +int ToRosData(Kinova::Api::Base::TrajectoryInfo input, kortex_driver::TrajectoryInfo &output); int ToRosData(Kinova::Api::Base::ActionExecutionState input, kortex_driver::ActionExecutionState &output); int ToRosData(Kinova::Api::Base::RobotEventNotification input, kortex_driver::RobotEventNotification &output); int ToRosData(Kinova::Api::Base::FactoryNotification input, kortex_driver::FactoryNotification &output); @@ -332,6 +348,7 @@ int ToRosData(Kinova::Api::Base::GripperCommand input, kortex_driver::GripperCom int ToRosData(Kinova::Api::Base::GripperRequest input, kortex_driver::GripperRequest &output); int ToRosData(Kinova::Api::Base::Gripper input, kortex_driver::Gripper &output); int ToRosData(Kinova::Api::Base::Finger input, kortex_driver::Finger &output); +int ToRosData(Kinova::Api::Base::GpioCommand input, kortex_driver::GpioCommand &output); int ToRosData(Kinova::Api::Base::SystemTime input, kortex_driver::SystemTime &output); int ToRosData(Kinova::Api::Base::ControllerConfigurationMode input, kortex_driver::ControllerConfigurationMode &output); int ToRosData(Kinova::Api::Base::ControllerConfiguration input, kortex_driver::ControllerConfiguration &output); @@ -349,7 +366,14 @@ int ToRosData(Kinova::Api::Base::PreComputedJointTrajectory input, kortex_driver int ToRosData(Kinova::Api::Base::PreComputedJointTrajectoryElement input, kortex_driver::PreComputedJointTrajectoryElement &output); int ToRosData(Kinova::Api::Base::TrajectoryErrorElement input, kortex_driver::TrajectoryErrorElement &output); int ToRosData(Kinova::Api::Base::TrajectoryErrorReport input, kortex_driver::TrajectoryErrorReport &output); +int ToRosData(Kinova::Api::Base::WaypointValidationReport input, kortex_driver::WaypointValidationReport &output); +int ToRosData(Kinova::Api::Base::Waypoint input, kortex_driver::Waypoint &output); +int ToRosData(Kinova::Api::Base::AngularWaypoint input, kortex_driver::AngularWaypoint &output); +int ToRosData(Kinova::Api::Base::CartesianWaypoint input, kortex_driver::CartesianWaypoint &output); +int ToRosData(Kinova::Api::Base::WaypointList input, kortex_driver::WaypointList &output); +int ToRosData(Kinova::Api::Base::KinematicTrajectoryConstraints input, kortex_driver::KinematicTrajectoryConstraints &output); int ToRosData(Kinova::Api::Base::FirmwareBundleVersions input, kortex_driver::FirmwareBundleVersions &output); int ToRosData(Kinova::Api::Base::FirmwareComponentVersion input, kortex_driver::FirmwareComponentVersion &output); +int ToRosData(Kinova::Api::Base::IKData input, kortex_driver::IKData &output); #endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/base_services.h b/kortex_driver/include/kortex_driver/generated/robot/base_services.h index 27c48320..499a811c 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/base_services.h +++ b/kortex_driver/include/kortex_driver/generated/robot/base_services.h @@ -91,7 +91,7 @@ class BaseRobotServices : public IBaseServices virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; - virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; + virtual bool Base_OnNotificationControlModeTopic(kortex_driver::Base_OnNotificationControlModeTopic::Request &req, kortex_driver::Base_OnNotificationControlModeTopic::Response &res) override; virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; @@ -183,12 +183,16 @@ class BaseRobotServices : public IBaseServices virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; + virtual bool ExecuteWaypointTrajectory(kortex_driver::ExecuteWaypointTrajectory::Request &req, kortex_driver::ExecuteWaypointTrajectory::Response &res) override; virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; + virtual bool ComputeForwardKinematics(kortex_driver::ComputeForwardKinematics::Request &req, kortex_driver::ComputeForwardKinematics::Response &res) override; + virtual bool ComputeInverseKinematics(kortex_driver::ComputeInverseKinematics::Request &req, kortex_driver::ComputeInverseKinematics::Response &res) override; + virtual bool ValidateWaypointList(kortex_driver::ValidateWaypointList::Request &req, kortex_driver::ValidateWaypointList::Response &res) override; private: uint32_t m_current_device_id; diff --git a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h index 6b19dd05..f64158de 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_proto_converter.h @@ -59,6 +59,7 @@ #include "kortex_driver/AngularTwist.h" #include "kortex_driver/ControlConfig_JointSpeeds.h" #include "kortex_driver/ControlConfig_ControlModeInformation.h" +#include "kortex_driver/ControlConfig_ControlModeNotification.h" int ToProtoData(kortex_driver::GravityVector input, Kinova::Api::ControlConfig::GravityVector *output); @@ -79,5 +80,6 @@ int ToProtoData(kortex_driver::LinearTwist input, Kinova::Api::ControlConfig::Li int ToProtoData(kortex_driver::AngularTwist input, Kinova::Api::ControlConfig::AngularTwist *output); int ToProtoData(kortex_driver::ControlConfig_JointSpeeds input, Kinova::Api::ControlConfig::JointSpeeds *output); int ToProtoData(kortex_driver::ControlConfig_ControlModeInformation input, Kinova::Api::ControlConfig::ControlModeInformation *output); +int ToProtoData(kortex_driver::ControlConfig_ControlModeNotification input, Kinova::Api::ControlConfig::ControlModeNotification *output); #endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h index 6fd389fb..ce93816b 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_ros_converter.h @@ -59,6 +59,7 @@ #include "kortex_driver/AngularTwist.h" #include "kortex_driver/ControlConfig_JointSpeeds.h" #include "kortex_driver/ControlConfig_ControlModeInformation.h" +#include "kortex_driver/ControlConfig_ControlModeNotification.h" int ToRosData(Kinova::Api::ControlConfig::GravityVector input, kortex_driver::GravityVector &output); @@ -79,5 +80,6 @@ int ToRosData(Kinova::Api::ControlConfig::LinearTwist input, kortex_driver::Line int ToRosData(Kinova::Api::ControlConfig::AngularTwist input, kortex_driver::AngularTwist &output); int ToRosData(Kinova::Api::ControlConfig::JointSpeeds input, kortex_driver::ControlConfig_JointSpeeds &output); int ToRosData(Kinova::Api::ControlConfig::ControlModeInformation input, kortex_driver::ControlConfig_ControlModeInformation &output); +int ToRosData(Kinova::Api::ControlConfig::ControlModeNotification input, kortex_driver::ControlConfig_ControlModeNotification &output); #endif \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h index e4adde31..6b396169 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/robot/controlconfig_services.h @@ -61,6 +61,8 @@ class ControlConfigRobotServices : public IControlConfigServices virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; + virtual bool ControlConfig_OnNotificationControlModeTopic(kortex_driver::ControlConfig_OnNotificationControlModeTopic::Request &req, kortex_driver::ControlConfig_OnNotificationControlModeTopic::Response &res) override; + virtual void cb_ControlModeTopic(Kinova::Api::ControlConfig::ControlModeNotification notif) override; private: uint32_t m_current_device_id; diff --git a/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h index 4b201c00..3e44c274 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_proto_converter.h @@ -44,7 +44,7 @@ #include "kortex_driver/EthernetDeviceIdentification.h" #include "kortex_driver/EthernetConfiguration.h" #include "kortex_driver/GPIOIdentification.h" -#include "kortex_driver/GPIOConfiguration.h" +#include "kortex_driver/InterconnectConfig_GPIOConfiguration.h" #include "kortex_driver/GPIOState.h" #include "kortex_driver/I2CDeviceIdentification.h" #include "kortex_driver/I2CConfiguration.h" @@ -58,7 +58,7 @@ int ToProtoData(kortex_driver::EthernetDeviceIdentification input, Kinova::Api::InterconnectConfig::EthernetDeviceIdentification *output); int ToProtoData(kortex_driver::EthernetConfiguration input, Kinova::Api::InterconnectConfig::EthernetConfiguration *output); int ToProtoData(kortex_driver::GPIOIdentification input, Kinova::Api::InterconnectConfig::GPIOIdentification *output); -int ToProtoData(kortex_driver::GPIOConfiguration input, Kinova::Api::InterconnectConfig::GPIOConfiguration *output); +int ToProtoData(kortex_driver::InterconnectConfig_GPIOConfiguration input, Kinova::Api::InterconnectConfig::GPIOConfiguration *output); int ToProtoData(kortex_driver::GPIOState input, Kinova::Api::InterconnectConfig::GPIOState *output); int ToProtoData(kortex_driver::I2CDeviceIdentification input, Kinova::Api::InterconnectConfig::I2CDeviceIdentification *output); int ToProtoData(kortex_driver::I2CConfiguration input, Kinova::Api::InterconnectConfig::I2CConfiguration *output); diff --git a/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h index f155cf65..1738af0d 100644 --- a/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h +++ b/kortex_driver/include/kortex_driver/generated/robot/interconnectconfig_ros_converter.h @@ -44,7 +44,7 @@ #include "kortex_driver/EthernetDeviceIdentification.h" #include "kortex_driver/EthernetConfiguration.h" #include "kortex_driver/GPIOIdentification.h" -#include "kortex_driver/GPIOConfiguration.h" +#include "kortex_driver/InterconnectConfig_GPIOConfiguration.h" #include "kortex_driver/GPIOState.h" #include "kortex_driver/I2CDeviceIdentification.h" #include "kortex_driver/I2CConfiguration.h" @@ -58,7 +58,7 @@ int ToRosData(Kinova::Api::InterconnectConfig::EthernetDeviceIdentification input, kortex_driver::EthernetDeviceIdentification &output); int ToRosData(Kinova::Api::InterconnectConfig::EthernetConfiguration input, kortex_driver::EthernetConfiguration &output); int ToRosData(Kinova::Api::InterconnectConfig::GPIOIdentification input, kortex_driver::GPIOIdentification &output); -int ToRosData(Kinova::Api::InterconnectConfig::GPIOConfiguration input, kortex_driver::GPIOConfiguration &output); +int ToRosData(Kinova::Api::InterconnectConfig::GPIOConfiguration input, kortex_driver::InterconnectConfig_GPIOConfiguration &output); int ToRosData(Kinova::Api::InterconnectConfig::GPIOState input, kortex_driver::GPIOState &output); int ToRosData(Kinova::Api::InterconnectConfig::I2CDeviceIdentification input, kortex_driver::I2CDeviceIdentification &output); int ToRosData(Kinova::Api::InterconnectConfig::I2CConfiguration input, kortex_driver::I2CConfiguration &output); diff --git a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h index 65566751..0d9e4826 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h @@ -146,8 +146,8 @@ class BaseSimulationServices : public IBaseServices std::function OnNotificationMappingInfoTopicHandler = nullptr; virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; - std::function OnNotificationControlModeTopicHandler = nullptr; - virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; + std::function Base_OnNotificationControlModeTopicHandler = nullptr; + virtual bool Base_OnNotificationControlModeTopic(kortex_driver::Base_OnNotificationControlModeTopic::Request &req, kortex_driver::Base_OnNotificationControlModeTopic::Response &res) override; virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; std::function OnNotificationOperatingModeTopicHandler = nullptr; virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; @@ -318,6 +318,8 @@ class BaseSimulationServices : public IBaseServices virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; std::function GetFirmwareBundleVersionsHandler = nullptr; virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; + std::function ExecuteWaypointTrajectoryHandler = nullptr; + virtual bool ExecuteWaypointTrajectory(kortex_driver::ExecuteWaypointTrajectory::Request &req, kortex_driver::ExecuteWaypointTrajectory::Response &res) override; std::function MoveSequenceTaskHandler = nullptr; virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; std::function DuplicateMappingHandler = nullptr; @@ -330,6 +332,12 @@ class BaseSimulationServices : public IBaseServices virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; std::function GetAllControllerConfigurationsHandler = nullptr; virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; + std::function ComputeForwardKinematicsHandler = nullptr; + virtual bool ComputeForwardKinematics(kortex_driver::ComputeForwardKinematics::Request &req, kortex_driver::ComputeForwardKinematics::Response &res) override; + std::function ComputeInverseKinematicsHandler = nullptr; + virtual bool ComputeInverseKinematics(kortex_driver::ComputeInverseKinematics::Request &req, kortex_driver::ComputeInverseKinematics::Response &res) override; + std::function ValidateWaypointListHandler = nullptr; + virtual bool ValidateWaypointList(kortex_driver::ValidateWaypointList::Request &req, kortex_driver::ValidateWaypointList::Response &res) override; }; #endif diff --git a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h index 30a8738a..0bfeb34a 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h @@ -87,6 +87,9 @@ class ControlConfigSimulationServices : public IControlConfigServices virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; std::function ResetJointAccelerationSoftLimitsHandler = nullptr; virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; + std::function ControlConfig_OnNotificationControlModeTopicHandler = nullptr; + virtual bool ControlConfig_OnNotificationControlModeTopic(kortex_driver::ControlConfig_OnNotificationControlModeTopic::Request &req, kortex_driver::ControlConfig_OnNotificationControlModeTopic::Response &res) override; + virtual void cb_ControlModeTopic(Kinova::Api::ControlConfig::ControlModeNotification notif) override; }; #endif diff --git a/kortex_driver/include/kortex_driver/non-generated/cartesian_trajectory_action_server.h b/kortex_driver/include/kortex_driver/non-generated/cartesian_trajectory_action_server.h new file mode 100644 index 00000000..65ac5648 --- /dev/null +++ b/kortex_driver/include/kortex_driver/non-generated/cartesian_trajectory_action_server.h @@ -0,0 +1,99 @@ +#ifndef _KORTEX_CARTESIAN_TRAJECTORY_ACTION_SERVER_H_ +#define _KORTEX_CARTESIAN_TRAJECTORY_ACTION_SERVER_H_ + +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2021 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +#include +#include +#include +#include + +#include "BaseClientRpc.h" +#include "BaseCyclicClientRpc.h" +#include "Errors.pb.h" +#include "kortex_driver/non-generated/kortex_math_util.h" +#include "kortex_driver/FollowCartesianTrajectoryAction.h" +#include "kortex_driver/FollowCartesianTrajectoryActionFeedback.h" +#include "kortex_driver/FollowCartesianTrajectoryActionGoal.h" +#include "kortex_driver/FollowCartesianTrajectoryActionResult.h" + +class CartesianTrajectoryActionServer +{ + public: + + enum ActionServerState + { + INITIALIZING = 0, + IDLE, + PRE_PROCESSING_PENDING, + PRE_PROCESSING_IN_PROGRESS, + TRAJECTORY_EXECUTION_PENDING, + TRAJECTORY_EXECUTION_IN_PROGRESS, + }; + + CartesianTrajectoryActionServer() = delete; + CartesianTrajectoryActionServer(const std::string& server_name, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic); + ~CartesianTrajectoryActionServer(); + + ActionServerState getState() {return m_server_state;}; + + const char* actionServerStateNames[int(ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS) + 1] = + { + "INITIALIZING", + "IDLE", + "PRE_PROCESSING_PENDING", + "PRE_PROCESSING_IN_PROGRESS", + "TRAJECTORY_EXECUTION_PENDING", + "TRAJECTORY_EXECUTION_IN_PROGRESS" + }; + + private: + // Members + ros::NodeHandle m_node_handle; + actionlib::ActionServer m_server; + + Kinova::Api::Common::NotificationHandle m_sub_action_notif_handle; + + uint32_t m_currentActionID; + + Kinova::Api::Base::BaseClient* m_base; + + std::string m_server_name; + + actionlib::ActionServer::GoalHandle m_goal; + std::chrono::system_clock::time_point m_trajectory_start_time; + std::chrono::system_clock::time_point m_trajectory_end_time; + + std::mutex m_server_state_lock; + std::mutex m_action_notification_thread_lock; + ActionServerState m_server_state; + + // ROS Params + std::string m_prefix; + + // Action Server Callbacks + void goal_received_callback(actionlib::ActionServer::GoalHandle new_goal_handle); + void preempt_received_callback(actionlib::ActionServer::GoalHandle goal_handle); + + // Kortex Notifications Callbacks + void action_notif_callback(Kinova::Api::Base::ActionNotification notif); + + // Private methods + bool is_goal_acceptable(actionlib::ActionServer::GoalHandle goal_handle); + bool is_goal_tolerance_respected(bool enable_prints, bool check_time_tolerance); + void stop_all_movement(); + + void set_server_state(ActionServerState s); +}; + +#endif //_KORTEX_CARTESIAN_TRAJECTORY_ACTION_SERVER_H_ \ No newline at end of file diff --git a/kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h b/kortex_driver/include/kortex_driver/non-generated/joint_trajectory_action_server.h similarity index 57% rename from kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h rename to kortex_driver/include/kortex_driver/non-generated/joint_trajectory_action_server.h index 85bcb3b5..dbdc6c21 100644 --- a/kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h +++ b/kortex_driver/include/kortex_driver/non-generated/joint_trajectory_action_server.h @@ -1,5 +1,5 @@ -#ifndef _KORTEX_PRE_COMPUTED_JOINT_TRAJECTORY_ACTION_SERVER_H_ -#define _KORTEX_PRE_COMPUTED_JOINT_TRAJECTORY_ACTION_SERVER_H_ +#ifndef _KORTEX_JOINT_TRAJECTORY_ACTION_SERVER_H_ +#define _KORTEX_JOINT_TRAJECTORY_ACTION_SERVER_H_ /* * KINOVA (R) KORTEX (TM) @@ -13,18 +13,38 @@ * */ +#include "kortex_driver/non-generated/kortex_math_util.h" + +#include +#include +#include +#include +#include + #include #include #include + +#include + #include #include -#include "BaseClientRpc.h" -#include "BaseCyclicClientRpc.h" -#include "Errors.pb.h" -#include "kortex_driver/non-generated/kortex_math_util.h" +struct AngularTrajectorySoftLimits +{ + Kinova::Api::ControlConfig::JointSpeedSoftLimits m_vel; + Kinova::Api::ControlConfig::JointAccelerationSoftLimits m_acc; + AngularTrajectorySoftLimits() = default; + AngularTrajectorySoftLimits(const Kinova::Api::ControlConfig::JointSpeedSoftLimits& vel, Kinova::Api::ControlConfig::JointAccelerationSoftLimits acc): + m_vel(vel), m_acc(acc) {} + inline bool empty() const + { + return google::protobuf::util::MessageDifferencer::Equals(m_vel, Kinova::Api::ControlConfig::JointSpeedSoftLimits::default_instance()) + && google::protobuf::util::MessageDifferencer::Equals(m_acc, Kinova::Api::ControlConfig::JointAccelerationSoftLimits::default_instance()); + } +}; -class PreComputedJointTrajectoryActionServer +class JointTrajectoryActionServer { public: @@ -38,9 +58,9 @@ class PreComputedJointTrajectoryActionServer TRAJECTORY_EXECUTION_IN_PROGRESS, }; - PreComputedJointTrajectoryActionServer() = delete; - PreComputedJointTrajectoryActionServer(const std::string& server_name, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic); - ~PreComputedJointTrajectoryActionServer(); + JointTrajectoryActionServer() = delete; + JointTrajectoryActionServer(const std::string& server_name, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic, Kinova::Api::ControlConfig::ControlConfigClient* control_config, bool use_hard_limits = false); + ~JointTrajectoryActionServer(); ActionServerState getState() {return m_server_state;}; @@ -61,8 +81,9 @@ class PreComputedJointTrajectoryActionServer Kinova::Api::Common::NotificationHandle m_sub_action_notif_handle; - Kinova::Api::Base::BaseClient* m_base; - Kinova::Api::BaseCyclic::BaseCyclicClient* m_base_cyclic; + Kinova::Api::Base::BaseClient* m_base; + Kinova::Api::BaseCyclic::BaseCyclicClient* m_base_cyclic; + Kinova::Api::ControlConfig::ControlConfigClient* m_control_config; std::string m_server_name; @@ -75,14 +96,17 @@ class PreComputedJointTrajectoryActionServer std::mutex m_action_notification_thread_lock; ActionServerState m_server_state; - KortexMathUtil m_math_util; - // ROS Params double m_default_goal_time_tolerance; double m_default_goal_tolerance; std::vector m_joint_names; std::string m_prefix; + // Limits handling + bool m_use_hard_limits; + AngularTrajectorySoftLimits m_soft_limits; + Kinova::Api::ControlConfig::KinematicLimits m_hard_limits; + // Action Server Callbacks void goal_received_callback(actionlib::ActionServer::GoalHandle new_goal_handle); void preempt_received_callback(actionlib::ActionServer::GoalHandle goal_handle); @@ -94,9 +118,13 @@ class PreComputedJointTrajectoryActionServer bool is_goal_acceptable(actionlib::ActionServer::GoalHandle goal_handle); bool is_goal_tolerance_respected(bool enable_prints, bool check_time_tolerance); void stop_all_movement(); + + AngularTrajectorySoftLimits getAngularTrajectorySoftLimits(); + void setAngularTrajectorySoftLimitsToMax(); + void setAngularTrajectorySoftLimits(const AngularTrajectorySoftLimits& limits); void set_server_state(ActionServerState s); }; -#endif +#endif //_KORTEX_JOINT_TRAJECTORY_ACTION_SERVER_H_ diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index e15da237..1c30e0f5 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -53,7 +53,8 @@ #include "kortex_driver/generated/simulation/visionconfig_services.h" #include "kortex_driver/generated/simulation/controlconfig_services.h" -#include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" +#include "kortex_driver/non-generated/joint_trajectory_action_server.h" +#include "kortex_driver/non-generated/cartesian_trajectory_action_server.h" #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" #include "kortex_driver/non-generated/kortex_subscribers.h" #include "kortex_driver/non-generated/kortex_arm_simulation.h" @@ -144,7 +145,8 @@ class KortexArmDriver IVisionConfigServices* m_vision_config_ros_services; // Action servers - PreComputedJointTrajectoryActionServer* m_action_server_follow_joint_trajectory; + JointTrajectoryActionServer* m_action_server_follow_joint_trajectory; + CartesianTrajectoryActionServer* m_action_server_follow_cartesian_trajectory; RobotiqGripperCommandActionServer* m_action_server_gripper_command; // Topic subscribers @@ -161,7 +163,6 @@ class KortexArmDriver // Private methods bool isGripperPresent(); - void setAngularTrajectorySoftLimitsToMax(); void publishRobotFeedback(); void publishSimulationFeedback(); void registerSimulationHandlers(); diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h index 21a0aa10..5f254856 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -76,7 +76,7 @@ class KortexArmSimulation KortexArmSimulation() = delete; KortexArmSimulation(ros::NodeHandle& nh); ~KortexArmSimulation(); - std::unordered_map GetActionsMap() const; + std::unordered_map GetActionsMap() const {return m_map_actions;} int GetDOF() const {return m_degrees_of_freedom;} kortex_driver::BaseCyclic_Feedback GetFeedback(); diff --git a/kortex_driver/msg/generated/SubErrorCodes.msg b/kortex_driver/msg/generated/SubErrorCodes.msg index a5797ae5..3e5f7e28 100644 --- a/kortex_driver/msg/generated/SubErrorCodes.msg +++ b/kortex_driver/msg/generated/SubErrorCodes.msg @@ -192,3 +192,9 @@ uint32 READ_PERMISSION_DENIED = 136 uint32 CONTROLLER_INVALID_MAPPING = 137 uint32 ACTION_IN_USE = 138 + +uint32 SEND_FAILED = 139 + +uint32 CONTROL_WAYPOINT_TRAJECTORY_ABORTED = 140 + +uint32 CONTROL_PERMISSION_DENIED = 141 diff --git a/kortex_driver/msg/generated/actuator_config/ActuatorConfig_ControlMode.msg b/kortex_driver/msg/generated/actuator_config/ActuatorConfig_ControlMode.msg index 7f80fdc4..53d2da2a 100644 --- a/kortex_driver/msg/generated/actuator_config/ActuatorConfig_ControlMode.msg +++ b/kortex_driver/msg/generated/actuator_config/ActuatorConfig_ControlMode.msg @@ -10,3 +10,5 @@ uint32 TORQUE = 3 uint32 CURRENT = 4 uint32 CUSTOM = 5 + +uint32 TORQUE_HIGH_VELOCITY = 6 diff --git a/kortex_driver/msg/generated/actuator_config/ControlLoopSelection.msg b/kortex_driver/msg/generated/actuator_config/ControlLoopSelection.msg index 6b620930..18daa4f1 100644 --- a/kortex_driver/msg/generated/actuator_config/ControlLoopSelection.msg +++ b/kortex_driver/msg/generated/actuator_config/ControlLoopSelection.msg @@ -12,3 +12,5 @@ uint32 MOTOR_VELOCITY = 8 uint32 JOINT_TORQUE = 16 uint32 MOTOR_CURRENT = 32 + +uint32 JOINT_TORQUE_HIGH_VELOCITY = 64 diff --git a/kortex_driver/msg/generated/base/ActionEvent.msg b/kortex_driver/msg/generated/base/ActionEvent.msg index 27e0889b..60142a9e 100644 --- a/kortex_driver/msg/generated/base/ActionEvent.msg +++ b/kortex_driver/msg/generated/base/ActionEvent.msg @@ -20,3 +20,5 @@ uint32 ACTION_POSTPROCESS_START = 8 uint32 ACTION_POSTPROCESS_ABORT = 9 uint32 ACTION_POSTPROCESS_END = 10 + +uint32 ACTION_FEEDBACK = 11 diff --git a/kortex_driver/msg/generated/base/ActionNotification.msg b/kortex_driver/msg/generated/base/ActionNotification.msg index 097c002c..7a01b0f7 100644 --- a/kortex_driver/msg/generated/base/ActionNotification.msg +++ b/kortex_driver/msg/generated/base/ActionNotification.msg @@ -4,4 +4,5 @@ ActionHandle handle Timestamp timestamp UserProfileHandle user_handle uint32 abort_details -Connection connection \ No newline at end of file +Connection connection +TrajectoryInfo[] trajectory_info \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/ActionType.msg b/kortex_driver/msg/generated/base/ActionType.msg index d9afad29..f178ff0e 100644 --- a/kortex_driver/msg/generated/base/ActionType.msg +++ b/kortex_driver/msg/generated/base/ActionType.msg @@ -37,6 +37,12 @@ uint32 EXECUTE_ACTION = 32 uint32 SEND_GRIPPER_COMMAND = 33 +uint32 SEND_GPIO_COMMAND = 34 + uint32 STOP_ACTION = 35 uint32 PLAY_PRE_COMPUTED_TRAJECTORY = 39 + +uint32 EXECUTE_SEQUENCE = 40 + +uint32 EXECUTE_WAYPOINT_LIST = 41 diff --git a/kortex_driver/msg/generated/base/Action_action_parameters.msg b/kortex_driver/msg/generated/base/Action_action_parameters.msg index a5a2853f..345dcd6b 100644 --- a/kortex_driver/msg/generated/base/Action_action_parameters.msg +++ b/kortex_driver/msg/generated/base/Action_action_parameters.msg @@ -17,5 +17,8 @@ Faults[] clear_faults Delay[] delay ActionHandle[] execute_action GripperCommand[] send_gripper_command +GpioCommand[] send_gpio_command Base_Stop[] stop_action -PreComputedJointTrajectory[] play_pre_computed_trajectory \ No newline at end of file +PreComputedJointTrajectory[] play_pre_computed_trajectory +SequenceHandle[] execute_sequence +WaypointList[] execute_waypoint_list \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/AngularWaypoint.msg b/kortex_driver/msg/generated/base/AngularWaypoint.msg new file mode 100644 index 00000000..ecddf61b --- /dev/null +++ b/kortex_driver/msg/generated/base/AngularWaypoint.msg @@ -0,0 +1,4 @@ + +float32[] angles +float32[] maximum_velocities +float32 duration \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/ControlModeNotification.msg b/kortex_driver/msg/generated/base/Base_ControlModeNotification.msg similarity index 100% rename from kortex_driver/msg/generated/base/ControlModeNotification.msg rename to kortex_driver/msg/generated/base/Base_ControlModeNotification.msg diff --git a/kortex_driver/msg/generated/base/Base_GpioConfiguration.msg b/kortex_driver/msg/generated/base/Base_GpioConfiguration.msg new file mode 100644 index 00000000..13c572ee --- /dev/null +++ b/kortex_driver/msg/generated/base/Base_GpioConfiguration.msg @@ -0,0 +1,3 @@ + +uint32 port_number +GpioPinConfiguration[] pin_configurations \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/CartesianWaypoint.msg b/kortex_driver/msg/generated/base/CartesianWaypoint.msg new file mode 100644 index 00000000..a4f6946a --- /dev/null +++ b/kortex_driver/msg/generated/base/CartesianWaypoint.msg @@ -0,0 +1,6 @@ + +Pose pose +uint32 reference_frame +float32 maximum_linear_velocity +float32 maximum_angular_velocity +float32 blending_radius \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/ControlModeNotificationList.msg b/kortex_driver/msg/generated/base/ControlModeNotificationList.msg index ac035c88..c873fe4a 100644 --- a/kortex_driver/msg/generated/base/ControlModeNotificationList.msg +++ b/kortex_driver/msg/generated/base/ControlModeNotificationList.msg @@ -1,2 +1,2 @@ -ControlModeNotification[] notifications \ No newline at end of file +Base_ControlModeNotification[] notifications \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/ControllerType.msg b/kortex_driver/msg/generated/base/ControllerType.msg index 538a75f1..d0ad7b9d 100644 --- a/kortex_driver/msg/generated/base/ControllerType.msg +++ b/kortex_driver/msg/generated/base/ControllerType.msg @@ -8,3 +8,5 @@ uint32 WRIST_CONTROLLER = 2 uint32 BASIC_JOYSTICK_CONTROLLER = 3 uint32 BASE_GPIO_CONTROLLER = 4 + +uint32 GPIO_JOYSTICK = 5 diff --git a/kortex_driver/msg/generated/base/GpioAction.msg b/kortex_driver/msg/generated/base/GpioAction.msg new file mode 100644 index 00000000..521588b4 --- /dev/null +++ b/kortex_driver/msg/generated/base/GpioAction.msg @@ -0,0 +1,10 @@ + +uint32 UNSPECIFIED_GPIO_ACTION = 0 + +uint32 GPIOACTION_SET = 1 + +uint32 GPIOACTION_CLEAR = 2 + +uint32 GPIOACTION_PULSE_HIGH = 3 + +uint32 GPIOACTION_PULSE_LOW = 4 diff --git a/kortex_driver/msg/generated/base/GpioBehavior.msg b/kortex_driver/msg/generated/base/GpioBehavior.msg index 08a5d62e..6afc0de2 100644 --- a/kortex_driver/msg/generated/base/GpioBehavior.msg +++ b/kortex_driver/msg/generated/base/GpioBehavior.msg @@ -8,3 +8,5 @@ uint32 GPIO_RISING = 2 uint32 GPIO_PULSE_LOW = 3 uint32 GPIO_PULSE_HIGH = 4 + +uint32 GPIO_ANALOG_CHANGE = 5 diff --git a/kortex_driver/msg/generated/base/GpioCommand.msg b/kortex_driver/msg/generated/base/GpioCommand.msg new file mode 100644 index 00000000..4781df87 --- /dev/null +++ b/kortex_driver/msg/generated/base/GpioCommand.msg @@ -0,0 +1,5 @@ + +uint32 port_identifier +uint32 pin_identifier +uint32 action +uint32 period \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/GpioConfigurationList.msg b/kortex_driver/msg/generated/base/GpioConfigurationList.msg new file mode 100644 index 00000000..7ff2f7a7 --- /dev/null +++ b/kortex_driver/msg/generated/base/GpioConfigurationList.msg @@ -0,0 +1,2 @@ + +Base_GpioConfiguration[] port_configurations \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/GpioPinConfiguration.msg b/kortex_driver/msg/generated/base/GpioPinConfiguration.msg new file mode 100644 index 00000000..f36cd59e --- /dev/null +++ b/kortex_driver/msg/generated/base/GpioPinConfiguration.msg @@ -0,0 +1,5 @@ + +uint32 pin_id +uint32 pin_property +bool output_enable +bool default_output_value \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/GpioPinPropertyFlags.msg b/kortex_driver/msg/generated/base/GpioPinPropertyFlags.msg new file mode 100644 index 00000000..d11ed688 --- /dev/null +++ b/kortex_driver/msg/generated/base/GpioPinPropertyFlags.msg @@ -0,0 +1,8 @@ + +uint32 GPIOPROPERTY_UNKNOWN = 0 + +uint32 GPIOPROPERTY_INPUT = 1 + +uint32 GPIOPROPERTY_OUTPUT = 2 + +uint32 GPIOPROPERTY_ANALOG = 4 diff --git a/kortex_driver/msg/generated/base/IKData.msg b/kortex_driver/msg/generated/base/IKData.msg new file mode 100644 index 00000000..d8304d96 --- /dev/null +++ b/kortex_driver/msg/generated/base/IKData.msg @@ -0,0 +1,3 @@ + +Pose cartesian_pose +JointAngles guess \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/KinematicTrajectoryConstraints.msg b/kortex_driver/msg/generated/base/KinematicTrajectoryConstraints.msg new file mode 100644 index 00000000..9e396094 --- /dev/null +++ b/kortex_driver/msg/generated/base/KinematicTrajectoryConstraints.msg @@ -0,0 +1,4 @@ + +float32[] angular_velocities +float32 linear_velocity +float32 angular_velocity \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/TrajectoryErrorElement.msg b/kortex_driver/msg/generated/base/TrajectoryErrorElement.msg index 3898803d..2f222397 100644 --- a/kortex_driver/msg/generated/base/TrajectoryErrorElement.msg +++ b/kortex_driver/msg/generated/base/TrajectoryErrorElement.msg @@ -5,4 +5,5 @@ float32 error_value float32 min_value float32 max_value uint32 index -string message \ No newline at end of file +string message +uint32 waypoint_index \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/TrajectoryErrorType.msg b/kortex_driver/msg/generated/base/TrajectoryErrorType.msg index 612e3b1c..e1320bcd 100644 --- a/kortex_driver/msg/generated/base/TrajectoryErrorType.msg +++ b/kortex_driver/msg/generated/base/TrajectoryErrorType.msg @@ -7,20 +7,30 @@ uint32 TRAJECTORY_ERROR_TYPE_ACTUATOR_COUNT_MISMATCH = 2 uint32 TRAJECTORY_ERROR_TYPE_INVALID_DURATION = 3 +uint32 TRAJECTORY_ERROR_TYPE_JOINT_NO_MOTION = 4 + uint32 TRAJECTORY_ERROR_TYPE_ZERO_DISTANCE = 4 +uint32 TRAJECTORY_ERROR_TYPE_INVALID_JOINT_SPEED = 5 + uint32 TRAJECTORY_ERROR_TYPE_INVALID_SPEED = 5 uint32 TRAJECTORY_ERROR_TYPE_LARGE_SPEED = 6 +uint32 TRAJECTORY_ERROR_TYPE_INVALID_JOINT_ACCELERATION = 7 + uint32 TRAJECTORY_ERROR_TYPE_INVALID_ACCELERATION = 7 uint32 TRAJECTORY_ERROR_TYPE_INVALID_TIME_STEP = 8 +uint32 TRAJECTORY_ERROR_TYPE_INVALID_TRAJECTORY_SIZE = 9 + uint32 TRAJECTORY_ERROR_TYPE_LARGE_SIZE = 9 uint32 TRAJECTORY_ERROR_TYPE_WRONG_MODE = 10 +uint32 TRAJECTORY_ERROR_TYPE_INVALID_JOINT_POSITION = 11 + uint32 TRAJECTORY_ERROR_TYPE_JOINT_POSITION_LIMIT = 11 uint32 TRAJECTORY_ERROR_TYPE_FILE_ERROR = 12 @@ -29,6 +39,8 @@ uint32 TRAJECTORY_ERROR_TYPE_NO_FILE_IN_MEMORY = 13 uint32 TRAJECTORY_ERROR_TYPE_INDEX_OUT_OF_TRAJ = 14 +uint32 TRAJECTORY_ERROR_TYPE_TRAJECTORY_ALREADY_RUNNING = 15 + uint32 TRAJECTORY_ERROR_TYPE_ALREADY_RUNNING = 15 uint32 TRAJECTORY_ERROR_TYPE_WRONG_STARTING_POINT = 16 @@ -38,3 +50,25 @@ uint32 TRAJECTORY_ERROR_TYPE_CARTESIAN_CANNOT_START = 17 uint32 TRAJECTORY_ERROR_TYPE_WRONG_STARTING_SPEED = 18 uint32 TRAJECTORY_ERROR_TYPE_INVALID_POSITION = 19 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_POSITION = 20 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_ORIENTATION = 21 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_LINEAR_VELOCITY = 22 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_ANGULAR_VELOCITY = 23 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_JOINT_TORQUE = 24 + +uint32 TRAJECTORY_ERROR_TYPE_MULTIPLE_WAYPOINT_TYPE_LIST = 25 + +uint32 TRAJECTORY_ERROR_TYPE_INITIAL_WAYPOINT_NO_STOP = 26 + +uint32 TRAJECTORY_ERROR_TYPE_FINAL_WAYPOINT_NO_STOP = 27 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_BLENDING_RADIUS = 28 + +uint32 TRAJECTORY_ERROR_TYPE_INVALID_REFERENCE_FRAME = 29 + +uint32 TRAJECTORY_ERROR_TYPE_NUMERICAL_ERROR_IMPOSSIBLE_TRAJECTORY = 30 diff --git a/kortex_driver/msg/generated/base/TrajectoryInfo.msg b/kortex_driver/msg/generated/base/TrajectoryInfo.msg new file mode 100644 index 00000000..48eceec9 --- /dev/null +++ b/kortex_driver/msg/generated/base/TrajectoryInfo.msg @@ -0,0 +1,4 @@ + +uint32 trajectory_info_type +uint32 waypoint_index +uint32 joint_index \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/TrajectoryInfoType.msg b/kortex_driver/msg/generated/base/TrajectoryInfoType.msg new file mode 100644 index 00000000..6a2998a8 --- /dev/null +++ b/kortex_driver/msg/generated/base/TrajectoryInfoType.msg @@ -0,0 +1,28 @@ + +uint32 UNSPECIFIED_TRAJECTORY_INFORMATION = 0 + +uint32 JOINT_ACCELERATION_LIMIT_REACHED = 1 + +uint32 JOINT_SPEED_LIMIT_REACHED = 2 + +uint32 JOINT_POSITION_LIMIT_REACHED = 3 + +uint32 JOINT_TORQUE_LIMIT_REACHED = 4 + +uint32 SINGULARITY_REGION = 5 + +uint32 INVERSE_KINEMATIC_FAILED = 6 + +uint32 CARTESIAN_ACCELERATION_LIMIT_REACHED = 7 + +uint32 CARTESIAN_SPEED_LIMIT_REACHED = 8 + +uint32 CARTESIAN_POSITION_LIMIT_REACHED = 9 + +uint32 CARTESIAN_WRENCH_LIMIT_REACHED = 10 + +uint32 ENTERING_PROTECTION_ZONE = 11 + +uint32 WAYPOINT_REACHED = 12 + +uint32 TRAJECTORY_OK = 13 diff --git a/kortex_driver/msg/generated/base/Waypoint.msg b/kortex_driver/msg/generated/base/Waypoint.msg new file mode 100644 index 00000000..c4a903db --- /dev/null +++ b/kortex_driver/msg/generated/base/Waypoint.msg @@ -0,0 +1,3 @@ + +string name +Waypoint_type_of_waypoint oneof_type_of_waypoint \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/WaypointList.msg b/kortex_driver/msg/generated/base/WaypointList.msg new file mode 100644 index 00000000..452b943f --- /dev/null +++ b/kortex_driver/msg/generated/base/WaypointList.msg @@ -0,0 +1,4 @@ + +Waypoint[] waypoints +float32 duration +bool use_optimal_blending \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/WaypointValidationReport.msg b/kortex_driver/msg/generated/base/WaypointValidationReport.msg new file mode 100644 index 00000000..58e0ae1c --- /dev/null +++ b/kortex_driver/msg/generated/base/WaypointValidationReport.msg @@ -0,0 +1,3 @@ + +TrajectoryErrorReport trajectory_error_report +WaypointList optimal_waypoint_list \ No newline at end of file diff --git a/kortex_driver/msg/generated/base/Waypoint_type_of_waypoint.msg b/kortex_driver/msg/generated/base/Waypoint_type_of_waypoint.msg new file mode 100644 index 00000000..fe4f7bb8 --- /dev/null +++ b/kortex_driver/msg/generated/base/Waypoint_type_of_waypoint.msg @@ -0,0 +1,3 @@ + +AngularWaypoint[] angular_waypoint +CartesianWaypoint[] cartesian_waypoint \ No newline at end of file diff --git a/kortex_driver/msg/generated/control_config/ControlConfig_ControlMode.msg b/kortex_driver/msg/generated/control_config/ControlConfig_ControlMode.msg index 0ded911c..67252bda 100644 --- a/kortex_driver/msg/generated/control_config/ControlConfig_ControlMode.msg +++ b/kortex_driver/msg/generated/control_config/ControlConfig_ControlMode.msg @@ -19,4 +19,6 @@ uint32 FORCE_CONTROL = 10 uint32 FORCE_CONTROL_MOTION_RESTRICTED = 11 +uint32 CARTESIAN_WAYPOINT_TRAJECTORY = 12 + uint32 IDLE = 13 diff --git a/kortex_driver/msg/generated/control_config/ControlConfig_ControlModeNotification.msg b/kortex_driver/msg/generated/control_config/ControlConfig_ControlModeNotification.msg new file mode 100644 index 00000000..802d67ad --- /dev/null +++ b/kortex_driver/msg/generated/control_config/ControlConfig_ControlModeNotification.msg @@ -0,0 +1,5 @@ + +uint32 control_mode +Timestamp timestamp +UserProfileHandle user_handle +Connection connection \ No newline at end of file diff --git a/kortex_driver/msg/generated/interconnect_config/GPIOConfiguration.msg b/kortex_driver/msg/generated/interconnect_config/InterconnectConfig_GPIOConfiguration.msg similarity index 100% rename from kortex_driver/msg/generated/interconnect_config/GPIOConfiguration.msg rename to kortex_driver/msg/generated/interconnect_config/InterconnectConfig_GPIOConfiguration.msg diff --git a/kortex_driver/package.xml b/kortex_driver/package.xml index 757f4407..fe4eb714 100644 --- a/kortex_driver/package.xml +++ b/kortex_driver/package.xml @@ -17,6 +17,7 @@ control_msgs controller_manager_msgs actionlib + actionlib_msgs moveit_ros_planning_interface kdl_parser roscpp @@ -25,8 +26,10 @@ roscpp rospy std_msgs + actionlib_msgs message_generation message_runtime + message_generation diff --git a/kortex_driver/protos/ActuatorConfig.proto b/kortex_driver/protos/ActuatorConfig.proto index 4072cc2d..206dfc8f 100644 --- a/kortex_driver/protos/ActuatorConfig.proto +++ b/kortex_driver/protos/ActuatorConfig.proto @@ -134,12 +134,13 @@ enum SafetyLimitType // Admissible control modes enum ControlMode { - NONE = 0; // None - POSITION = 1; // Position mode - VELOCITY = 2; // Velocity mode - TORQUE = 3; // Torque mode - CURRENT = 4; // Current mode - CUSTOM = 5; // Custom mode + NONE = 0; // None + POSITION = 1; // Position mode + VELOCITY = 2; // Velocity mode + TORQUE = 3; // Torque mode + CURRENT = 4; // Current mode + CUSTOM = 5; // Custom mode + TORQUE_HIGH_VELOCITY = 6; // High velocity torque mode } // Admissible command modes @@ -156,13 +157,14 @@ enum CommandMode // Admissible control loop selections enum ControlLoopSelection { - RESERVED = 0; // 0x0 - Reserved (internal use only) - JOINT_POSITION = 1; // 0x1 - Joint position control (if available) - MOTOR_POSITION = 2; // 0x2 - Motor position control - JOINT_VELOCITY = 4; // 0x4 - Joint velocity control (if available) - MOTOR_VELOCITY = 8; // 0x8 - Motor velocity control - JOINT_TORQUE = 16; // 0x10 - Joint torque control - MOTOR_CURRENT = 32; // 0x20 - Motor current control + RESERVED = 0; // 0x0 - Reserved (internal use only) + JOINT_POSITION = 1; // 0x1 - Joint position control (if available) + MOTOR_POSITION = 2; // 0x2 - Motor position control + JOINT_VELOCITY = 4; // 0x4 - Joint velocity control (if available) + MOTOR_VELOCITY = 8; // 0x8 - Motor velocity control + JOINT_TORQUE = 16; // 0x10 - Joint torque control + MOTOR_CURRENT = 32; // 0x20 - Motor current control + JOINT_TORQUE_HIGH_VELOCITY = 64; // 0x30 - High velocity joint torque control } // Admissable cogging feedforward modes @@ -176,14 +178,14 @@ enum CoggingFeedforwardMode // Axis position message AxisPosition { - float position = 1; // Axis position (meters) + float position = 1; // Axis position (degrees) } // Axis offsets message AxisOffsets { - float absolute_offset = 1; // Absolute offset value (meters) - float relative_offset = 2; // Relative offset value (meters) + float absolute_offset = 1; // Absolute offset value (degrees) + float relative_offset = 2; // Relative offset value (degrees) } // Torque calibration settings diff --git a/kortex_driver/protos/Base.proto b/kortex_driver/protos/Base.proto index 87a0517f..bbe09379 100644 --- a/kortex_driver/protos/Base.proto +++ b/kortex_driver/protos/Base.proto @@ -203,7 +203,7 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error rpc MappingInfoTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=99 @PUB_SUB=MappingInfoNotification // Subscribes to control mode topic for notifications - rpc ControlModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=100 @PUB_SUB=ControlModeNotification + rpc ControlModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=100 @PUB_SUB=ControlModeNotification @DEPRECATED="This function may be removed in a future release. It has been moved to ControlConfig service." // Subscribes to operating mode topic for notifications rpc OperatingModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=101 @PUB_SUB=OperatingModeNotification @@ -226,14 +226,17 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error // Subscribes to robot event topic for notifications rpc RobotEventTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=107 @PUB_SUB=RobotEventNotification - // Moves robot to the specified tool pose (position and orientation) while imposing specified constraints. - rpc PlayCartesianTrajectory (ConstrainedPose) returns (Kinova.Api.Common.Empty);//@RPC_ID=109 + // Moves robot to the specifed tool pose (position and orientation) while imposing specified constraints. + // This RPC will be deprecated in a future version and will be replaced by ExecuteWaypointTrajectory. + rpc PlayCartesianTrajectory (ConstrainedPose) returns (Kinova.Api.Common.Empty);//@RPC_ID=109 @DEPRECATED - // Moves robot to the specified position while imposing specified constraints. - rpc PlayCartesianTrajectoryPosition (ConstrainedPosition) returns (Kinova.Api.Common.Empty);//@RPC_ID=110 + // Moves robot to the specifed position while imposing specified constraints. + // This RPC will be deprecated in a future version and will be replaced by ExecuteWaypointTrajectory. + rpc PlayCartesianTrajectoryPosition (ConstrainedPosition) returns (Kinova.Api.Common.Empty);//@RPC_ID=110 @DEPRECATED - // Moves to the specified orientation while imposing specified constraints. - rpc PlayCartesianTrajectoryOrientation (ConstrainedOrientation) returns (Kinova.Api.Common.Empty);//@RPC_ID=111 + // Moves to the specifed orientation while imposing specified constraints. + // This RPC will be deprecated in a future version and will be replaced by ExecuteWaypointTrajectory. + rpc PlayCartesianTrajectoryOrientation (ConstrainedOrientation) returns (Kinova.Api.Common.Empty);//@RPC_ID=111 @DEPRECATED // Stops robot movement rpc Stop (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=112 @@ -260,10 +263,12 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error rpc SendTwistCommand (TwistCommand) returns (Kinova.Api.Common.Empty);//@RPC_ID=121 // Moves joints to the specified joint angles while imposing specified constraints. - rpc PlayJointTrajectory (ConstrainedJointAngles) returns (Kinova.Api.Common.Empty);//@RPC_ID=124 + // This RPC will be deprecated in a future version and will be replaced by ExecuteWaypointTrajectory. + rpc PlayJointTrajectory (ConstrainedJointAngles) returns (Kinova.Api.Common.Empty);//@RPC_ID=124 @DEPRECATED // Moves specifed joint to the specifed joint angle while imposing specified constraints. - rpc PlaySelectedJointTrajectory (ConstrainedJointAngle) returns (Kinova.Api.Common.Empty);//@RPC_ID=125 + // This RPC will be deprecated in a future version and will be replaced by ExecuteWaypointTrajectory. + rpc PlaySelectedJointTrajectory (ConstrainedJointAngle) returns (Kinova.Api.Common.Empty);//@RPC_ID=125 @DEPRECATED // Retrieves the currently measured joint angles for each joint rpc GetMeasuredJointAngles (Kinova.Api.Common.Empty) returns (JointAngles);//@RPC_ID=126 @@ -296,7 +301,7 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error rpc ClearFaults (Kinova.Api.Common.Empty) returns (Kinova.Api.Common.Empty);//@RPC_ID=146 // Retrieves current control mode - rpc GetControlMode (Kinova.Api.Common.Empty) returns (ControlModeInformation);//@RPC_ID=150 + rpc GetControlMode (Kinova.Api.Common.Empty) returns (ControlModeInformation);//@RPC_ID=150 @DEPRECATED="This function may be removed in a future release. It has been moved to ControlConfig service." // Retrieves current operating mode rpc GetOperatingMode (Kinova.Api.Common.Empty) returns (OperatingModeInformation);//@RPC_ID=151 @@ -464,6 +469,9 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error // Retrieves current firmware bundle versions rpc GetFirmwareBundleVersions (Kinova.Api.Common.Empty) returns (FirmwareBundleVersions);//@RPC_ID=224 + // Executes a trajectory defined by a series of waypoints in joint space or in Cartesian space + rpc ExecuteWaypointTrajectory (WaypointList) returns (Kinova.Api.Common.Empty);//@RPC_ID=226 + // Move task to new index in a sequence rpc MoveSequenceTask (SequenceTasksPair) returns (Kinova.Api.Common.Empty);//@RPC_ID=227 @@ -481,6 +489,46 @@ service Base {//@PROXY_ID=2 @ERROR=Kinova.Api.Error // Retrieves all controller configurations rpc GetAllControllerConfigurations (Kinova.Api.Common.Empty) returns (ControllerConfigurationList);//@RPC_ID=232 + + // Get the forward kinematics given specified angular positions of actuators + rpc ComputeForwardKinematics (JointAngles) returns (Pose);//@RPC_ID=233 + + // Get the inverse kinematics given a specified cartesian pose and guess of joint angles + rpc ComputeInverseKinematics (IKData) returns (JointAngles);//@RPC_ID=234 + + /* + * Validate a waypoint list, returns an empty trajectory error report list if the waypoint list is valid. + * If the use_optimal_blending option is true, a waypoint list with optimal blending will be return. + */ + rpc ValidateWaypointList (WaypointList) returns (WaypointValidationReport);//@RPC_ID=235 +} + +// List of GPIO port configurations +message GpioConfigurationList { + repeated GpioConfiguration port_configurations = 1; // Port configuration list +} + +// GPIO port configuration information +message GpioConfiguration { + uint32 port_number = 1; // Port number (Base extension port is port 0) + repeated GpioPinConfiguration pin_configurations = 2; // Pin configuration list +} + +// GPIO pin configuration information +message GpioPinConfiguration { + uint32 pin_id = 1; // Pin identifier + GpioPinPropertyFlags pin_property = 2; // Pin property (read only) + bool output_enable = 3; // Pin is configured as output if set to TRUE. If output is enabled, input events are masked. + bool default_output_value = 4; // Default output pin value. This is the value set when pin is initialized (TRUE == high / FALSE == low). +} + +// Admissible gpio pin properties +enum GpioPinPropertyFlags +{ + GPIOPROPERTY_UNKNOWN = 0; + GPIOPROPERTY_INPUT = 1; // 0x1 : Pin can be used as digital input + GPIOPROPERTY_OUTPUT = 2; // 0x2 : Pin can be used as digital output + GPIOPROPERTY_ANALOG = 4; // 0x4 : Pin can be used as analog input } // Identifies Base service current version @@ -621,8 +669,8 @@ message Action { TwistCommand send_twist_command = 4; // Control the tool in velocity WrenchCommand send_wrench_command = 5; // Control the tool in force (EXPERIMENTAL) JointSpeeds send_joint_speeds = 7; // Action to control each joint speed - ConstrainedPose reach_pose = 9; // Reach a pose given Cartesian constraints. - ConstrainedJointAngles reach_joint_angles = 10; // Reach a series of joint angles given angular constraints. + ConstrainedPose reach_pose = 9; // Reach a pose given Cartesian constraints. This action will be deprecated in a future version and will be replaced by execute_waypoint_list. + ConstrainedJointAngles reach_joint_angles = 10; // Reach a series of joint angles given angular constraints. This action will be deprecated in a future version and will be replaced by execute_waypoint_list. AdmittanceMode toggle_admittance_mode = 16; // Enable or disable the admittance mode Snapshot snapshot = 17; // Take a snapshot of current robot position SwitchControlMapping switch_control_mapping = 19; // Switch the active controller map @@ -636,8 +684,11 @@ message Action { Delay delay = 34; // Apply a delay ActionHandle execute_action = 35; // Execute an existing action GripperCommand send_gripper_command = 36; // Send a gripper command + GpioCommand send_gpio_command = 37; // Send a gpio command (not implemented yet) Stop stop_action = 38; // Stop movement PreComputedJointTrajectory play_pre_computed_trajectory = 39; // Play a pre-computed joint trajectory + SequenceHandle execute_sequence = 40; // Execute an existing sequence + WaypointList execute_waypoint_list = 41; // Execute a trajectory defined by a series of waypoints } } @@ -662,8 +713,11 @@ enum ActionType { TIME_DELAY = 31; // Apply a delay EXECUTE_ACTION = 32; // Execute an existing action SEND_GRIPPER_COMMAND = 33; // Send a gripper command + SEND_GPIO_COMMAND = 34; // Send a gpio command (not implemented yet) STOP_ACTION = 35; // Stop robot movement PLAY_PRE_COMPUTED_TRAJECTORY = 39; // Play a pre-computed trajectory + EXECUTE_SEQUENCE = 40; // Execute an existing sequence + EXECUTE_WAYPOINT_LIST = 41; // Execute a trajectory defined by a series of waypoints } // Admissible types of snapshots @@ -744,11 +798,11 @@ message CommunicationInterfaceConfiguration { // Admissible network types enum NetworkType { - UNSPECIFIED_NETWORK_TYPE = 0; // Unspecified network type - WIFI = 1; // Wi-Fi network - WIRED_ETHERNET = 2; // Wired Ethernet network - WIRED_MICROUSB = 3; // Wired Ethernet over USB network (RNDIS) - WIRED_USB = 4; // Wired Ethernet over USB network + UNSPECIFIED_NETWORK_TYPE = 0; // Unspecified network type + WIFI = 1; // Wi-Fi network + WIRED_ETHERNET = 2; // Wired Ethernet network + WIRED_MICROUSB = 3; // Wired Ethernet over USB network (RNDIS) + WIRED_USB = 4 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. } // Reference to a network @@ -1144,7 +1198,8 @@ enum ControllerType { XBOX_CONTROLLER = 1; // Xbox gamepad WRIST_CONTROLLER = 2; // Wrist buttons BASIC_JOYSTICK_CONTROLLER = 3; // Simplified joystick connected to Kinova robot base - BASE_GPIO_CONTROLLER = 4; // GPIO Controller (not implemented yet) + BASE_GPIO_CONTROLLER = 4; // GPIO Controller + GPIO_JOYSTICK = 5; // TEMP, will be deleted } // Reference to a specific controller device @@ -1220,6 +1275,7 @@ enum ActionEvent { ACTION_POSTPROCESS_START = 8; // Action post-process started ACTION_POSTPROCESS_ABORT = 9; // Action post-process aborted ACTION_POSTPROCESS_END = 10; // Action post-process ended + ACTION_FEEDBACK = 11; // Action feedback received } // Notification about a single action event @@ -1230,8 +1286,16 @@ message ActionNotification { Kinova.Api.Common.UserProfileHandle user_handle = 4; // User that caused the action event SubErrorCodes abort_details = 5; // Details if action_event is equal to ACTION_ABORT Kinova.Api.Common.Connection connection = 6; // Connection that caused the action event + repeated TrajectoryInfo trajectory_info = 7; // Additional information from the current action } +// Additional trajectory information +message TrajectoryInfo { + TrajectoryInfoType trajectory_info_type = 1; // Trajectory information type + uint32 waypoint_index = 2; // Waypoint index (if applicable) + uint32 joint_index = 3; // Joint index (if applicable) +} + // Indicates the execution state of an action (not implemented yet) message ActionExecutionState { ActionEvent action_event = 1; // Action event type @@ -1401,7 +1465,7 @@ message ControllerEvent { uint32 input_identifier = 3; // Controller input that caused the event } -// A GPIO event (not implemented yet) +// A GPIO event message GpioEvent { ControllerInputType input_type = 1; // Type of controller input that caused the event_identifier GpioBehavior behavior = 2; // GPIO behavior that occured @@ -1412,7 +1476,7 @@ message GpioEvent { message MapEvent { oneof events { SafetyEvent safety_event = 1; // Mapped safety event (not implemented yet) - GpioEvent gpio_event = 2; // Mapped GPIO event (not implemented yet) + GpioEvent gpio_event = 2; // Mapped GPIO event ControllerEvent controller_event = 3; // Mapped controller event } string name = 4; // Map event friendly name @@ -1774,6 +1838,22 @@ message Finger { float value = 2; } +// A command to control expansion port's GPIO +message GpioCommand { + uint32 port_identifier = 1; // Gpio port identifier (0 == base expansion port) + uint32 pin_identifier = 2; // Gpio pin identifier + GpioAction action = 3; // Action to perform on gpio + uint32 period = 4; // Period, in ms, of GPIO action (applicable only for pulse commands) +} + +// Admissible GPIO actions +enum GpioAction { + UNSPECIFIED_GPIO_ACTION = 0; // Unspecified gpio action + GPIOACTION_SET = 1; // Set GPIO output + GPIOACTION_CLEAR = 2; // Clear GPIO output + GPIOACTION_PULSE_HIGH = 3; // Pulse high GPIO output with specified period + GPIOACTION_PULSE_LOW = 4; // Pulse low GPIO output with specified period +} // Admissible map navigation directions enum NavigationDirection { @@ -1806,16 +1886,17 @@ enum LedState { LED_ON = 3; // LED is one } -// Admissible GPIO behavior (not implemented yet) +// Admissible GPIO behavior enum GpioBehavior { UNSPECIFIED_GPIO_BEHAVIOR = 0; // Unspecified GPIO behavior GPIO_FALLING = 1; // Falling edge GPIO_RISING = 2; // Rising edge GPIO_PULSE_LOW = 3; // Sequence of HIGH - LOW - HIGH GPIO_PULSE_HIGH = 4; // Sequence of LOW - HIGH - LOW + GPIO_ANALOG_CHANGE = 5; // Analog input value change } -// Available GPIO PIN (not implemented yet) +// Available GPIO PIN (See the user guide at section Base expansion connector) enum Gen3GpioPinId { UNSPECIFIED_PIN = 0; // Unspecified PIN ID GPIO_PIN_B = 1; // GPIO PIN B @@ -2015,49 +2096,80 @@ enum TrajectoryContinuityMode // Trajectory validation error types enum TrajectoryErrorType { + option allow_alias = true; TRAJECTORY_ERROR_TYPE_UNSPECIFIED = 0; // Unspecified error type - TRAJECTORY_ERROR_TYPE_OUTSIDE_WORKSPACE = 1; // The trajectory point is outside robot workspace - TRAJECTORY_ERROR_TYPE_ACTUATOR_COUNT_MISMATCH = 2; // There is an actuator count mismatch with the robot - TRAJECTORY_ERROR_TYPE_INVALID_DURATION = 3; // The trajectory has an invalid duration - TRAJECTORY_ERROR_TYPE_ZERO_DISTANCE = 4; // The trajectory does not move the robot because the delta is either zero or too small - TRAJECTORY_ERROR_TYPE_INVALID_SPEED = 5; // The speed for a trajectory point for an actuator is invalid - TRAJECTORY_ERROR_TYPE_LARGE_SPEED = 6; // The speed for a trajectory point for an actuator exceeds its limit - TRAJECTORY_ERROR_TYPE_INVALID_ACCELERATION = 7; // The acceleration for a trajectory point for an actuator is invalid - TRAJECTORY_ERROR_TYPE_INVALID_TIME_STEP = 8; // The time step for a trajectory point is invalid - TRAJECTORY_ERROR_TYPE_LARGE_SIZE = 9; // The trajectory is too large - TRAJECTORY_ERROR_TYPE_WRONG_MODE = 10; // The robot is not currently in Trajectory Control mode - TRAJECTORY_ERROR_TYPE_JOINT_POSITION_LIMIT = 11; // The position limit for a trajectory point for an actuator is exceeded - TRAJECTORY_ERROR_TYPE_FILE_ERROR = 12; // An internal file error was encountered - TRAJECTORY_ERROR_TYPE_NO_FILE_IN_MEMORY = 13; // An internal file memory error was encountered - TRAJECTORY_ERROR_TYPE_INDEX_OUT_OF_TRAJ = 14; // The index for a trajectory point is invalid - TRAJECTORY_ERROR_TYPE_ALREADY_RUNNING = 15; // A trajectory is already running - TRAJECTORY_ERROR_TYPE_WRONG_STARTING_POINT = 16; // The difference between the trajectory's starting point and the current position is too large - TRAJECTORY_ERROR_TYPE_CARTESIAN_CANNOT_START = 17; // The cartesian trajectory is not able to start - TRAJECTORY_ERROR_TYPE_WRONG_STARTING_SPEED = 18; // The difference between the trajectory's starting speed and the current speed is too large - TRAJECTORY_ERROR_TYPE_INVALID_POSITION = 19; // The position for a trajectory point for an actuator is invalid -} - -// Trajectory validation error identifiers + TRAJECTORY_ERROR_TYPE_OUTSIDE_WORKSPACE = 1; // The desired pose is outside robot workspace + TRAJECTORY_ERROR_TYPE_ACTUATOR_COUNT_MISMATCH = 2; // The provided number of joint values does not match robot's number of actuators + TRAJECTORY_ERROR_TYPE_INVALID_DURATION = 3; // The duration exceeds a limit or is invalid + + TRAJECTORY_ERROR_TYPE_JOINT_NO_MOTION = 4; // The robot's actuators are already at the targeted configuration + TRAJECTORY_ERROR_TYPE_ZERO_DISTANCE = 4 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_INVALID_JOINT_SPEED = 5; // The joint speed exceeds a limit, initial speed does not match current robot speed or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_SPEED = 5 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + TRAJECTORY_ERROR_TYPE_LARGE_SPEED = 6 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_INVALID_JOINT_ACCELERATION = 7; // The joint acceleration exceeds a limit or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_ACCELERATION = 7 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_INVALID_TIME_STEP = 8; // The time step does not match robot time step or is invalid + + TRAJECTORY_ERROR_TYPE_INVALID_TRAJECTORY_SIZE = 9; // The trajectory size is outside the limits or is invalid + TRAJECTORY_ERROR_TYPE_LARGE_SIZE = 9 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_WRONG_MODE = 10 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_INVALID_JOINT_POSITION = 11; // The joint position exceeds a limit, initial joint position does not match current robot joint position or is invalid + TRAJECTORY_ERROR_TYPE_JOINT_POSITION_LIMIT = 11 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_FILE_ERROR = 12 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + TRAJECTORY_ERROR_TYPE_NO_FILE_IN_MEMORY = 13 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + TRAJECTORY_ERROR_TYPE_INDEX_OUT_OF_TRAJ = 14 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_TRAJECTORY_ALREADY_RUNNING = 15; // The new trajectory cannot start because another trajectory is being played + TRAJECTORY_ERROR_TYPE_ALREADY_RUNNING = 15 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + TRAJECTORY_ERROR_TYPE_WRONG_STARTING_POINT = 16 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + TRAJECTORY_ERROR_TYPE_CARTESIAN_CANNOT_START = 17 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + TRAJECTORY_ERROR_TYPE_WRONG_STARTING_SPEED = 18 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + TRAJECTORY_ERROR_TYPE_INVALID_POSITION = 19 [deprecated=true]; // This enum value is deprecated and will be removed in a future release. + + + TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_POSITION = 20; // The cartesian position exceeds a limit, initial pose does not match current robot pose or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_ORIENTATION = 21; // The cartesian orientation exceeds a limit, initial orientation does not match current robot orientation or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_LINEAR_VELOCITY = 22; // The cartesian linear velocity exceeds a limit or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_CARTESIAN_ANGULAR_VELOCITY = 23; // The cartesian angular velocity exceeds a limit or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_JOINT_TORQUE = 24; // The joint torque exceeds a limit or is invalid + TRAJECTORY_ERROR_TYPE_MULTIPLE_WAYPOINT_TYPE_LIST = 25; // The waypoints in a trajectory must be all of the same type (cartesian or angular) + TRAJECTORY_ERROR_TYPE_INITIAL_WAYPOINT_NO_STOP = 26; // The first waypoint must not have blending (currently unused) + TRAJECTORY_ERROR_TYPE_FINAL_WAYPOINT_NO_STOP = 27; // The last waypoint must not have blending + TRAJECTORY_ERROR_TYPE_INVALID_BLENDING_RADIUS = 28; // The blending radius must be positive and not overlap other blendings or waypoint or is invalid + TRAJECTORY_ERROR_TYPE_INVALID_REFERENCE_FRAME = 29; // The reference frame is invalid or not supported + TRAJECTORY_ERROR_TYPE_NUMERICAL_ERROR_IMPOSSIBLE_TRAJECTORY = 30; // The conditions (points, velocities) of the trajectory lead to numerical errors that make the computation impossible +} + +// This enum is deprecated and will be removed in a future release enum TrajectoryErrorIdentifier { - TRAJECTORY_ERROR_IDENTIFIER_UNSPECIFIED = 0; // Unspecified error identifier - TRAJECTORY_ERROR_IDENTIFIER_UNAPPLICABLE = 1; // No identifier required for this error - TRAJECTORY_ERROR_IDENTIFIER_TIME = 2; // Time validation failed - TRAJECTORY_ERROR_IDENTIFIER_POSITION = 3; // Position validation failed - TRAJECTORY_ERROR_IDENTIFIER_VELOCITY = 4; // Velocity validation failed - TRAJECTORY_ERROR_IDENTIFIER_ACCELERATION = 5; // Acceleration validation failed + TRAJECTORY_ERROR_IDENTIFIER_UNSPECIFIED = 0; // Unspecified error identifier. This enum is deprecated. + TRAJECTORY_ERROR_IDENTIFIER_UNAPPLICABLE = 1; // No identifier required for this error. This enum is deprecated. + TRAJECTORY_ERROR_IDENTIFIER_TIME = 2; // Time validation failed. This enum is deprecated. + TRAJECTORY_ERROR_IDENTIFIER_POSITION = 3; // Position validation failed. This enum is deprecated. + TRAJECTORY_ERROR_IDENTIFIER_VELOCITY = 4; // Velocity validation failed. This enum is deprecated. + TRAJECTORY_ERROR_IDENTIFIER_ACCELERATION = 5; // Acceleration validation failed. This enum is deprecated. } // Details for a single trajectory validation error message TrajectoryErrorElement { TrajectoryErrorType error_type = 1; // Error type - TrajectoryErrorIdentifier error_identifier = 2; // Error identifier - float error_value = 3; // Erroneous value - float min_value = 4; // Minimum permitted value - float max_value = 5; // Maximum permitted value - uint32 index = 6; // Actuator index - string message = 7; // Clarification message for the error + TrajectoryErrorIdentifier error_identifier = 2 [deprecated = true]; // This field is deprecated and will be removed in a future release. + float error_value = 3; // Erroneous value + float min_value = 4; // Minimum permitted value + float max_value = 5; // Maximum permitted value + uint32 index = 6; // Actuator index + string message = 7; // Clarification message for the error + uint32 waypoint_index = 8; // Waypoint index (if it applies) } // Report collecting information on different validation errors for a particular trajectory @@ -2066,6 +2178,52 @@ message TrajectoryErrorReport repeated TrajectoryErrorElement trajectory_error_elements = 1; } +// Waypoint Validation results +message WaypointValidationReport +{ + TrajectoryErrorReport trajectory_error_report = 1; // Report on the validation, the waypoint list is valid if empty + WaypointList optimal_waypoint_list = 2; // Validated Waypoint List with optimal blending radius if the option was set +} + +// A waypoint describing part of a trajectory. +message Waypoint { + string name = 1; // Waypoint friendly name + oneof type_of_waypoint { // Oneof to define the waypoint type + AngularWaypoint angular_waypoint = 2; + CartesianWaypoint cartesian_waypoint = 3; + } +} + +// An angular Waypoint +message AngularWaypoint { + repeated float angles = 1; // Target position (in deg) + repeated float maximum_velocities = 2; // Maximum velocities for each actuator (in deg/s) during movement (optional) + float duration = 3; // Duration to reach this waypoint from the previous position (in seconds) +} + +// A Cartesian Waypoint +message CartesianWaypoint { + Pose pose = 1; // Target Cartesian Pose + Kinova.Api.Common.CartesianReferenceFrame reference_frame = 2; // The reference frame used for the goal Pose + float maximum_linear_velocity = 3; // Maximum linear velocity (in m/s) during movement (optional) + float maximum_angular_velocity = 4; // Maximum angular velocity (in deg/s) during movement (optional) + float blending_radius = 5; // Blending radius (in m) to use for the movement (if this waypoint is not an endpoint). +} + +// A waypoint list +message WaypointList { + repeated Waypoint waypoints = 1; // Array of waypoints + float duration = 2; // Duration of the waypoint list (in seconds). If unspecified or equal to 0, optimal duration is assumed. + bool use_optimal_blending = 3; // At validation if this value is true, the waypoint list with optimal blending will be returned. +} + +// Angular and Cartesian kinematic constraints (maximum velocities) +message KinematicTrajectoryConstraints { + repeated float angular_velocities = 1; // Angular velocities for each actuator (in deg/s) + float linear_velocity = 2; // Linear velocity (in m/s) + float angular_velocity = 3; // Angular velocity (in deg/s) +} + // Firmware bundle versions including main firmware bundle version and components versions message FirmwareBundleVersions { @@ -2080,3 +2238,29 @@ message FirmwareComponentVersion string version = 2; // Version of the component uint32 device_id = 3; // Device id of the component } + +// Input needed for the calculation of inverse kinematics +message IKData +{ + Pose cartesian_pose = 1; // Cartesian pose of the end effector used to calculate the corresponding joint angles + JointAngles guess = 2; // Initial guess for the joint angles +} + +// Additional trajectory info type +enum TrajectoryInfoType +{ + UNSPECIFIED_TRAJECTORY_INFORMATION = 0; // No information provided + JOINT_ACCELERATION_LIMIT_REACHED = 1; // Joint acceleration limit reached + JOINT_SPEED_LIMIT_REACHED = 2; // Joint speed limit reached + JOINT_POSITION_LIMIT_REACHED = 3; // Joint position limit reached + JOINT_TORQUE_LIMIT_REACHED = 4; // Joint torque limit reached + SINGULARITY_REGION = 5; // The arm is inside a singularity region + INVERSE_KINEMATIC_FAILED = 6; // The inverse kinematic calculation has failed + CARTESIAN_ACCELERATION_LIMIT_REACHED = 7; // Cartesian acceleration limit reached + CARTESIAN_SPEED_LIMIT_REACHED = 8; // Cartesian speed limit reached + CARTESIAN_POSITION_LIMIT_REACHED = 9; // Cartesian position limit reached + CARTESIAN_WRENCH_LIMIT_REACHED = 10; // Cartesian torque limit reached + ENTERING_PROTECTION_ZONE = 11; // The arm is entering a protection zone + WAYPOINT_REACHED = 12; // Waypoint reached + TRAJECTORY_OK = 13; // There is more trajectory notification +} diff --git a/kortex_driver/protos/ControlConfig.proto b/kortex_driver/protos/ControlConfig.proto index 55a50c40..18107152 100644 --- a/kortex_driver/protos/ControlConfig.proto +++ b/kortex_driver/protos/ControlConfig.proto @@ -25,7 +25,7 @@ service ControlConfig {//@PROXY_ID=16 @ERROR=Kinova.Api.Error // Retrieves global gravity vector rpc GetGravityVector (Kinova.Api.Common.Empty) returns (GravityVector);//@RPC_ID=2 - // Sets payload information. This needs to be configured so that the control library can take into account the presence of the payload mass in computing the dynamics of the robot. + // Sets payload information. This needs to be configured so that the control library can take into account the presence of the payload mass in computing the dynamics of the robot. rpc SetPayloadInformation (PayloadInformation) returns (Kinova.Api.Common.Empty);//@RPC_ID=3 // Retrieves payload information @@ -49,7 +49,7 @@ service ControlConfig {//@PROXY_ID=16 @ERROR=Kinova.Api.Error // Retrieves the current reference frame used by the twist and wrench commands rpc GetCartesianReferenceFrame (Kinova.Api.Common.Empty) returns (CartesianReferenceFrameInfo);//@RPC_ID=10 - // Retrieves current control mode (not implemented yet) + // Retrieves current control mode rpc GetControlMode (Kinova.Api.Common.Empty) returns (ControlModeInformation);//@RPC_ID=13 // Set the software joint speed limits. @@ -72,7 +72,7 @@ service ControlConfig {//@PROXY_ID=16 @ERROR=Kinova.Api.Error // Retrieves the software kinematic limits for all control modes. rpc GetAllKinematicSoftLimits (Kinova.Api.Common.Empty) returns (KinematicLimitsList);//@RPC_ID=20 - + // Set the desired linear twist when using the joystick. rpc SetDesiredLinearTwist (LinearTwist) returns (Kinova.Api.Common.Empty);//@RPC_ID=21 @@ -85,7 +85,7 @@ service ControlConfig {//@PROXY_ID=16 @ERROR=Kinova.Api.Error // Retrieves the desired joystick speeds rpc GetDesiredSpeeds (Kinova.Api.Common.Empty) returns (DesiredSpeeds);//@RPC_ID=24 - // Resets gravity vector to default values + // Resets gravity vector to default values rpc ResetGravityVector (Kinova.Api.Common.Empty) returns (GravityVector);//@RPC_ID=25 // Resets payload information to default values @@ -105,6 +105,9 @@ service ControlConfig {//@PROXY_ID=16 @ERROR=Kinova.Api.Error // Resets joint acceleration soft limits to default values rpc ResetJointAccelerationSoftLimits (ControlModeInformation) returns (JointAccelerationSoftLimits);//@RPC_ID=31 + + // Subscribes to control mode topic for notifications + rpc ControlModeTopic (Kinova.Api.Common.NotificationOptions) returns (Kinova.Api.Common.NotificationHandle);//@RPC_ID=32 @PUB_SUB=ControlModeNotification } // Identifies ControlConfig current version @@ -201,7 +204,7 @@ message TwistLinearSoftLimit { // Software twist angular speed limit message TwistAngularSoftLimit { ControlMode control_mode = 1; // Control mode - float twist_angular_soft_limit = 2; // Software angular twist limit + float twist_angular_soft_limit = 2; // Software angular twist limit } // Software joint speed limits @@ -221,7 +224,7 @@ message KinematicLimits { ControlMode control_mode = 1; // Control mode float twist_linear = 2; // Linear twist limits float twist_angular = 3; // Angular twist limits - repeated float joint_speed_limits = 4; // Joint speed limits + repeated float joint_speed_limits = 4; // Joint speed limits repeated float joint_acceleration_limits = 5; // Joint Acceleration limits } @@ -230,24 +233,24 @@ message KinematicLimitsList { repeated KinematicLimits kinematic_limits_list = 1; // List of kinematic limits } -// Desired Joystick speeds. +// Desired Joystick speeds. message DesiredSpeeds { float linear = 1; // Desired linear speed (meters / second) float angular = 2; // Desired angular speed (degrees / second) repeated float joint_speed = 3; // Desired joint speeds (degrees / second) } -// Desired Joystick linear speed. +// Desired Joystick linear speed. message LinearTwist { float linear = 1; // Desired linear speed (meters / second) } -// Desired Joystick angular speed. +// Desired Joystick angular speed. message AngularTwist { float angular = 1; // Desired angular speed (degrees / second) } -// Desired Joystick joint speeds. +// Desired Joystick joint speeds. message JointSpeeds { repeated float joint_speed = 1; // Desired joint speeds (degrees / second) } @@ -264,6 +267,7 @@ enum ControlMode { NULL_SPACE_ADMITTANCE = 8; // Null space mode FORCE_CONTROL = 10; // Force control mode FORCE_CONTROL_MOTION_RESTRICTED = 11; // Force control motion restricted mode + CARTESIAN_WAYPOINT_TRAJECTORY = 12; // Cartesian waypoint trajectory mode IDLE = 13; // Idle }; @@ -271,3 +275,11 @@ enum ControlMode { message ControlModeInformation { ControlMode control_mode = 1; // Control mode } + +// Notification about a single control mode event +message ControlModeNotification { + ControlMode control_mode = 1; // New control mode + Kinova.Api.Common.Timestamp timestamp = 2; // Event timestamp + Kinova.Api.Common.UserProfileHandle user_handle = 3; // User that caused the control mode event + Kinova.Api.Common.Connection connection = 4; // Connection that caused the control mode event +} diff --git a/kortex_driver/protos/Errors.proto b/kortex_driver/protos/Errors.proto index dad290ac..229dc021 100644 --- a/kortex_driver/protos/Errors.proto +++ b/kortex_driver/protos/Errors.proto @@ -149,4 +149,10 @@ enum SubErrorCodes { CONTROLLER_INVALID_MAPPING = 137; // Attempting to assign an unsuited mapping to controller ACTION_IN_USE = 138; // Attempting to delete an Action used by another entity + + SEND_FAILED = 139; // An error occured when trying to send the message (Transport error) + + CONTROL_WAYPOINT_TRAJECTORY_ABORTED = 140; // Waypoint trajectory sequence aborted by kontrol library + + CONTROL_PERMISSION_DENIED = 141; // Permission to execute a routine from the kontrol interface denied } diff --git a/kortex_driver/protos/GripperCyclicMessage.proto b/kortex_driver/protos/GripperCyclicMessage.proto index c27ac093..55cc60bf 100644 --- a/kortex_driver/protos/GripperCyclicMessage.proto +++ b/kortex_driver/protos/GripperCyclicMessage.proto @@ -23,10 +23,10 @@ message MessageId { // Command to a single gripper motor message MotorCommand { - fixed32 motor_id = 1; // Motor ID (1, nb_motor) - float position = 3; // Desired position of the gripper fingers in percentage (0-100%) - float velocity = 4; // Desired velocity in percentage (0-100%) with which position will be set - float force = 5; // Desired force limit of the gripper fingers in percentage (0-100%) + fixed32 motor_id = 1; // Motor ID (1, nb_motor) + float position = 3; // Desired position of the gripper fingers in percentage (0-100%) + float velocity = 4; // Desired velocity in percentage (0-100%) with which position will be set + float force = 5 [deprecated = true]; // This field is deprecated and unused. It will be removed in a future release. } // Command sent to a gripper diff --git a/kortex_driver/scripts/ros_kortex_generator.py b/kortex_driver/scripts/ros_kortex_generator.py index 8d46ad33..4fcfdc74 100755 --- a/kortex_driver/scripts/ros_kortex_generator.py +++ b/kortex_driver/scripts/ros_kortex_generator.py @@ -249,7 +249,7 @@ class DetailedRPC: - prepend_on_notification: If the RPC is a Notification RPC, is put to "OnNotification". Otherwise, it is empty [string] - notification_message_cpp_namespace: Full C++ namespace of the message that corresponds to the Notification RPC ("SafetyNotification" for the RPC "SafetyTopic") [string] ''' - def __init__(self, rpc, package, is_rpc_duplicated, is_rpc_deprecated, is_input_type_duplicated, is_output_type_duplicated, notification_message=None): + def __init__(self, rpc, package, is_rpc_duplicated, is_rpc_deprecated, is_input_type_duplicated, is_output_type_duplicated, notification_message_cpp_namespace=None): ''' Constructor for the DetailedRPC @@ -260,7 +260,6 @@ def __init__(self, rpc, package, is_rpc_duplicated, is_rpc_deprecated, is_input_ - is_rpc_deprecated: True if the message type is deprecated in the C++ Kortex API [bool] - is_input_type_duplicated: True if the input type of the RPC is duplicated in the generation context [bool] - is_output_type_duplicated: True if the output type of the RPC is duplicated in the generation context [bool] - - notification_message: Protobuf object for the Notification message that corresponds to the Notification RPC (optional) [DescrptorProto] ''' self.name = rpc.name self.name_lowercase_with_underscores = '_'.join(re.findall('[A-Z][^A-Z]*', self.name)).lower() @@ -296,12 +295,17 @@ def __init__(self, rpc, package, is_rpc_duplicated, is_rpc_deprecated, is_input_ # Notifications have the word 'Topic' at the end of the RPC name self.is_notification_rpc = re.match(r"\w+Topic", rpc.name) - if self.is_notification_rpc: - self.prepend_on_notification = "OnNotification" - self.notification_message_cpp_namespace = notification_message[0].cpp_namespace - else: - self.prepend_on_notification = "" - self.notification_message_cpp_namespace = "" + self.notification_message_cpp_namespace = "" + self.prepend_on_notification = "OnNotification" if self.is_notification_rpc else "" + + def set_notification_cpp_namespace(self, notification_message_cpp_namespace): + ''' + Setter for the DetailedRPC's notification namespace + + Arguments: + - notification_message_cpp_namespace: C++ namespace for the Notification message that goes with the Notification RPC (optional) [string] + ''' + self.notification_message_cpp_namespace = notification_message_cpp_namespace # Jinja2 function to render a file from a template def render(tpl_path, context): @@ -320,15 +324,18 @@ def generate_code(request, response): # Create an ordered dictionary for all Protobuf packages packages_dict = OrderedDict() - # Find the *Notification messages to add them to a list - notification_messages_list = [] - # Find Messages and RPC's that have the same name within the proto files because ROS doesn't handle namespaces messages_unordered_set = set() rpcs_unordered_set = set() duplicated_messages_unordered_set = set() duplicated_rpcs_unordered_set = set() deprecated_rpcs_dict_of_lists = OrderedDict() + + # Find the *Notification messages to match them with their On*Topic RPCs + # key example: "Kinova.Api.Package.MyGivenTopic" + # value example: "Kinova.Api.Package.MyGivenNotification" + notification_messages_map = {} + for filename, proto_file in file_map.items(): # Traverse proto files with the recursive calls for item, package in traverse(proto_file): @@ -340,13 +347,10 @@ def generate_code(request, response): packages_dict[package] = DetailedPackage(package) # If the item is a message or an enum if not isinstance(item, ServiceDescriptorProto): - if item.name not in messages_unordered_set: - messages_unordered_set.add(item.name) - # Add the notifications to a list to match them to their respective RPCs later (because there package may differ from the RPC's) - if re.match(r"\w+Notification", item.name): - notification_messages_list.append(DetailedMessage(item, package, False)) + if item.name.casefold() not in messages_unordered_set: + messages_unordered_set.add(item.name.casefold()) else: - duplicated_messages_unordered_set.add(item.name) + duplicated_messages_unordered_set.add(item.name.casefold()) # If the item is a Protobuf service (a collection of methods) else: for method in item.method: @@ -364,9 +368,21 @@ def generate_code(request, response): while text_line != "": text_line = proto_file_reader.readline() if "rpc " in text_line: + # Find deprecation tags if re.search(r'@DEPRECATED', text_line): rpc_name = re.sub(RPC_REGULAR_EXPRESSION, r'\1', text_line).strip() deprecated_rpcs_dict_of_lists[filename_without_extension].append(rpc_name) + # Find notification tags + m = re.search(r"(\w+Topic).*@PUB_SUB=(.*Notification)", text_line) + if m: + # Name is already complete + if "." in m.group(2): + notif = m.group(2) + # Name is relative to current package + else: + notif = "{}.{}".format(package, m.group(2)) + key = "{}.{}".format(package, m.group(1)) + notification_messages_map[key] = notif # print (deprecated_rpcs_dict_of_lists.items()) @@ -398,13 +414,13 @@ def generate_code(request, response): # If this is an enum if isinstance(item, EnumDescriptorProto): - is_enum_duplicated = item.name in duplicated_messages_unordered_set + is_enum_duplicated = item.name.casefold() in duplicated_messages_unordered_set current_package.addEnum(DetailedMessage(item, package, is_enum_duplicated)) # If this is a message if isinstance(item, DescriptorProto): - is_message_duplicated = item.name in duplicated_messages_unordered_set - duplicated_fields = filter(lambda x : x.type_name.split(".")[-1] in duplicated_messages_unordered_set, item.field) + is_message_duplicated = item.name.casefold() in duplicated_messages_unordered_set + duplicated_fields = filter(lambda x : x.type_name.split(".")[-1].casefold() in duplicated_messages_unordered_set, item.field) temp_message = DetailedMessage(item, package, is_message_duplicated, duplicated_fields) # Find if the message contains oneof message_contains_one_of = False @@ -425,7 +441,7 @@ def generate_code(request, response): for member in item.field: # If a member is part of a one_of, add it to the DetailedOneOf it belongs to if member.HasField("oneof_index"): - is_field_duplicated = member.type_name.split(".")[-1] in duplicated_messages_unordered_set + is_field_duplicated = member.type_name.split(".")[-1].casefold() in duplicated_messages_unordered_set temp_message.one_of_list[member.oneof_index].addField(member, is_field_duplicated) current_package.addMessage(temp_message) @@ -440,14 +456,17 @@ def generate_code(request, response): if rpc.name in FORBIDDEN_RPC_METHODS: continue is_rpc_duplicated = rpc.name in duplicated_rpcs_unordered_set - is_input_type_duplicated = rpc.input_type.split(".")[-1] in duplicated_messages_unordered_set - is_output_type_duplicated = rpc.output_type.split(".")[-1] in duplicated_messages_unordered_set - notification_message = list(filter(lambda x: x.name == rpc.name.replace("Topic", "Notification") , notification_messages_list)) - - # Find out if this RPC is deprecated + is_input_type_duplicated = rpc.input_type.split(".")[-1].casefold() in duplicated_messages_unordered_set + is_output_type_duplicated = rpc.output_type.split(".")[-1].casefold() in duplicated_messages_unordered_set is_rpc_deprecated = True if rpc.name in deprecated_rpcs_in_this_service else False + temp_rpc = DetailedRPC(rpc, package, is_rpc_duplicated, is_rpc_deprecated, is_input_type_duplicated, is_output_type_duplicated) + + # Add Notification C++ namespace to RPC if it is a notification RPC + if temp_rpc.is_notification_rpc: + notification_message = notification_messages_map["{}.{}".format(package, rpc.name)] + notification_message_cpp_namespace = "::".join(notification_message.split('.')[:-1]) + temp_rpc.set_notification_cpp_namespace(notification_message_cpp_namespace) - temp_rpc = DetailedRPC(rpc, package, is_rpc_duplicated, is_rpc_deprecated, is_input_type_duplicated, is_output_type_duplicated, notification_message) current_package.addRPC(temp_rpc) ########################################### diff --git a/kortex_driver/scripts/tests/launch_test.py b/kortex_driver/scripts/tests/launch_test.py index 4abdd9fc..82358a3f 100644 --- a/kortex_driver/scripts/tests/launch_test.py +++ b/kortex_driver/scripts/tests/launch_test.py @@ -29,8 +29,7 @@ command.append("ip_address:={}".format(args.ip)) command.append("start_moveit:=true") elif args.test_case == 'gazebo': - command.append("rostest") - command.append("--reuse-master") + command.append("roslaunch") command.append("kortex_gazebo") command.append("spawn_kortex_robot.launch") command.append("gazebo_gui:=false") @@ -52,16 +51,19 @@ time.sleep(3) # If we want to run the tests for the driver if args.test_case == 'driver': - subprocess.run(command) + p = subprocess.Popen(command) + p.wait() # If we want to test the examples elif args.test_case == 'examples': - driver_roslaunch_process = subprocess.Popen(command, stdout=subprocess.DEVNULL) + p = subprocess.Popen(command, stdout=subprocess.DEVNULL) print ("Starting the ROS Kortex driver...") time.sleep(10) examples_tests_process = subprocess.Popen(['rostest', '--reuse-master', 'kortex_examples', 'run_all_examples.launch', 'robot_name:=my_{}'.format(args.arm)]) examples_tests_process.wait() elif args.test_case == 'gazebo': - subprocess.run(command) - + p = subprocess.Popen(command) + p.wait() +except KeyboardInterrupt: + p.terminate() finally: os.killpg(os.getpid(), signal.SIGTERM) diff --git a/kortex_driver/src/generated/robot/base_proto_converter.cpp b/kortex_driver/src/generated/robot/base_proto_converter.cpp index 1cefcb7b..b5ae899e 100644 --- a/kortex_driver/src/generated/robot/base_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/base_proto_converter.cpp @@ -16,6 +16,39 @@ #include "kortex_driver/generated/robot/base_proto_converter.h" +int ToProtoData(kortex_driver::GpioConfigurationList input, Kinova::Api::Base::GpioConfigurationList *output) +{ + + output->clear_port_configurations(); + for(int i = 0; i < input.port_configurations.size(); i++) + { + ToProtoData(input.port_configurations[i], output->add_port_configurations()); + } + + return 0; +} +int ToProtoData(kortex_driver::Base_GpioConfiguration input, Kinova::Api::Base::GpioConfiguration *output) +{ + + output->set_port_number(input.port_number); + output->clear_pin_configurations(); + for(int i = 0; i < input.pin_configurations.size(); i++) + { + ToProtoData(input.pin_configurations[i], output->add_pin_configurations()); + } + + return 0; +} +int ToProtoData(kortex_driver::GpioPinConfiguration input, Kinova::Api::Base::GpioPinConfiguration *output) +{ + + output->set_pin_id(input.pin_id); + output->set_pin_property((Kinova::Api::Base::GpioPinPropertyFlags)input.pin_property); + output->set_output_enable(input.output_enable); + output->set_default_output_value(input.default_output_value); + + return 0; +} int ToProtoData(kortex_driver::FullUserProfile input, Kinova::Api::Base::FullUserProfile *output) { @@ -274,6 +307,10 @@ int ToProtoData(kortex_driver::Action input, Kinova::Api::Base::Action *output) { ToProtoData(input.oneof_action_parameters.send_gripper_command[0], output->mutable_send_gripper_command()); } + if(input.oneof_action_parameters.send_gpio_command.size() > 0) + { + ToProtoData(input.oneof_action_parameters.send_gpio_command[0], output->mutable_send_gpio_command()); + } if(input.oneof_action_parameters.stop_action.size() > 0) { ToProtoData(input.oneof_action_parameters.stop_action[0], output->mutable_stop_action()); @@ -282,6 +319,14 @@ int ToProtoData(kortex_driver::Action input, Kinova::Api::Base::Action *output) { ToProtoData(input.oneof_action_parameters.play_pre_computed_trajectory[0], output->mutable_play_pre_computed_trajectory()); } + if(input.oneof_action_parameters.execute_sequence.size() > 0) + { + ToProtoData(input.oneof_action_parameters.execute_sequence[0], output->mutable_execute_sequence()); + } + if(input.oneof_action_parameters.execute_waypoint_list.size() > 0) + { + ToProtoData(input.oneof_action_parameters.execute_waypoint_list[0], output->mutable_execute_waypoint_list()); + } return 0; } @@ -680,7 +725,7 @@ int ToProtoData(kortex_driver::Base_ControlModeInformation input, Kinova::Api::B return 0; } -int ToProtoData(kortex_driver::ControlModeNotification input, Kinova::Api::Base::ControlModeNotification *output) +int ToProtoData(kortex_driver::Base_ControlModeNotification input, Kinova::Api::Base::ControlModeNotification *output) { output->set_control_mode((Kinova::Api::Base::ControlMode)input.control_mode); @@ -854,7 +899,21 @@ int ToProtoData(kortex_driver::ActionNotification input, Kinova::Api::Base::Acti ToProtoData(input.timestamp, output->mutable_timestamp()); ToProtoData(input.user_handle, output->mutable_user_handle()); output->set_abort_details((Kinova::Api::SubErrorCodes)input.abort_details); - ToProtoData(input.connection, output->mutable_connection()); + ToProtoData(input.connection, output->mutable_connection()); + output->clear_trajectory_info(); + for(int i = 0; i < input.trajectory_info.size(); i++) + { + ToProtoData(input.trajectory_info[i], output->add_trajectory_info()); + } + + return 0; +} +int ToProtoData(kortex_driver::TrajectoryInfo input, Kinova::Api::Base::TrajectoryInfo *output) +{ + + output->set_trajectory_info_type((Kinova::Api::Base::TrajectoryInfoType)input.trajectory_info_type); + output->set_waypoint_index(input.waypoint_index); + output->set_joint_index(input.joint_index); return 0; } @@ -1482,6 +1541,16 @@ int ToProtoData(kortex_driver::Finger input, Kinova::Api::Base::Finger *output) return 0; } +int ToProtoData(kortex_driver::GpioCommand input, Kinova::Api::Base::GpioCommand *output) +{ + + output->set_port_identifier(input.port_identifier); + output->set_pin_identifier(input.pin_identifier); + output->set_action((Kinova::Api::Base::GpioAction)input.action); + output->set_period(input.period); + + return 0; +} int ToProtoData(kortex_driver::SystemTime input, Kinova::Api::Base::SystemTime *output) { @@ -1647,6 +1716,7 @@ int ToProtoData(kortex_driver::TrajectoryErrorElement input, Kinova::Api::Base:: output->set_max_value(input.max_value); output->set_index(input.index); output->set_message(input.message); + output->set_waypoint_index(input.waypoint_index); return 0; } @@ -1661,6 +1731,83 @@ int ToProtoData(kortex_driver::TrajectoryErrorReport input, Kinova::Api::Base::T return 0; } +int ToProtoData(kortex_driver::WaypointValidationReport input, Kinova::Api::Base::WaypointValidationReport *output) +{ + + ToProtoData(input.trajectory_error_report, output->mutable_trajectory_error_report()); + ToProtoData(input.optimal_waypoint_list, output->mutable_optimal_waypoint_list()); + + return 0; +} +int ToProtoData(kortex_driver::Waypoint input, Kinova::Api::Base::Waypoint *output) +{ + + output->set_name(input.name); + if(input.oneof_type_of_waypoint.angular_waypoint.size() > 0) + { + ToProtoData(input.oneof_type_of_waypoint.angular_waypoint[0], output->mutable_angular_waypoint()); + } + if(input.oneof_type_of_waypoint.cartesian_waypoint.size() > 0) + { + ToProtoData(input.oneof_type_of_waypoint.cartesian_waypoint[0], output->mutable_cartesian_waypoint()); + } + + return 0; +} +int ToProtoData(kortex_driver::AngularWaypoint input, Kinova::Api::Base::AngularWaypoint *output) +{ + + output->clear_angles(); + for(int i = 0; i < input.angles.size(); i++) + { + output->add_angles(input.angles[i]); + } + output->clear_maximum_velocities(); + for(int i = 0; i < input.maximum_velocities.size(); i++) + { + output->add_maximum_velocities(input.maximum_velocities[i]); + } + output->set_duration(input.duration); + + return 0; +} +int ToProtoData(kortex_driver::CartesianWaypoint input, Kinova::Api::Base::CartesianWaypoint *output) +{ + + ToProtoData(input.pose, output->mutable_pose()); + output->set_reference_frame((Kinova::Api::Common::CartesianReferenceFrame)input.reference_frame); + output->set_maximum_linear_velocity(input.maximum_linear_velocity); + output->set_maximum_angular_velocity(input.maximum_angular_velocity); + output->set_blending_radius(input.blending_radius); + + return 0; +} +int ToProtoData(kortex_driver::WaypointList input, Kinova::Api::Base::WaypointList *output) +{ + + output->clear_waypoints(); + for(int i = 0; i < input.waypoints.size(); i++) + { + ToProtoData(input.waypoints[i], output->add_waypoints()); + } + output->set_duration(input.duration); + output->set_use_optimal_blending(input.use_optimal_blending); + + return 0; +} +int ToProtoData(kortex_driver::KinematicTrajectoryConstraints input, Kinova::Api::Base::KinematicTrajectoryConstraints *output) +{ + + output->clear_angular_velocities(); + for(int i = 0; i < input.angular_velocities.size(); i++) + { + output->add_angular_velocities(input.angular_velocities[i]); + } + output->set_linear_velocity(input.linear_velocity); + output->set_angular_velocity(input.angular_velocity); + + return 0; +} int ToProtoData(kortex_driver::FirmwareBundleVersions input, Kinova::Api::Base::FirmwareBundleVersions *output) { @@ -1682,3 +1829,11 @@ int ToProtoData(kortex_driver::FirmwareComponentVersion input, Kinova::Api::Base return 0; } +int ToProtoData(kortex_driver::IKData input, Kinova::Api::Base::IKData *output) +{ + + ToProtoData(input.cartesian_pose, output->mutable_cartesian_pose()); + ToProtoData(input.guess, output->mutable_guess()); + + return 0; +} diff --git a/kortex_driver/src/generated/robot/base_ros_converter.cpp b/kortex_driver/src/generated/robot/base_ros_converter.cpp index 6ac426c1..1daed506 100644 --- a/kortex_driver/src/generated/robot/base_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/base_ros_converter.cpp @@ -16,6 +16,49 @@ #include "kortex_driver/generated/robot/base_ros_converter.h" +int ToRosData(Kinova::Api::Base::GpioConfigurationList input, kortex_driver::GpioConfigurationList &output) +{ + + output.port_configurations.clear(); + for(int i = 0; i < input.port_configurations_size(); i++) + { + decltype(output.port_configurations)::value_type temp; + ToRosData(input.port_configurations(i), temp); + output.port_configurations.push_back(temp); + } + + + + return 0; +} +int ToRosData(Kinova::Api::Base::GpioConfiguration input, kortex_driver::Base_GpioConfiguration &output) +{ + + output.port_number = input.port_number(); + output.pin_configurations.clear(); + for(int i = 0; i < input.pin_configurations_size(); i++) + { + decltype(output.pin_configurations)::value_type temp; + ToRosData(input.pin_configurations(i), temp); + output.pin_configurations.push_back(temp); + } + + + + return 0; +} +int ToRosData(Kinova::Api::Base::GpioPinConfiguration input, kortex_driver::GpioPinConfiguration &output) +{ + + output.pin_id = input.pin_id(); + output.pin_property = input.pin_property(); + output.output_enable = input.output_enable(); + output.default_output_value = input.default_output_value(); + + + + return 0; +} int ToRosData(Kinova::Api::Base::FullUserProfile input, kortex_driver::FullUserProfile &output) { @@ -45,7 +88,7 @@ int ToRosData(Kinova::Api::Base::UserProfileList input, kortex_driver::UserProfi output.user_profiles.clear(); for(int i = 0; i < input.user_profiles_size(); i++) { - kortex_driver::UserProfile temp; + decltype(output.user_profiles)::value_type temp; ToRosData(input.user_profiles(i), temp); output.user_profiles.push_back(temp); } @@ -60,7 +103,7 @@ int ToRosData(Kinova::Api::Base::UserList input, kortex_driver::UserList &output output.user_handles.clear(); for(int i = 0; i < input.user_handles_size(); i++) { - kortex_driver::UserProfileHandle temp; + decltype(output.user_handles)::value_type temp; ToRosData(input.user_handles(i), temp); output.user_handles.push_back(temp); } @@ -127,7 +170,7 @@ int ToRosData(Kinova::Api::Base::SequenceTasks input, kortex_driver::SequenceTas output.sequence_tasks.clear(); for(int i = 0; i < input.sequence_tasks_size(); i++) { - kortex_driver::SequenceTask temp; + decltype(output.sequence_tasks)::value_type temp; ToRosData(input.sequence_tasks(i), temp); output.sequence_tasks.push_back(temp); } @@ -143,7 +186,7 @@ int ToRosData(Kinova::Api::Base::SequenceTasksConfiguration input, kortex_driver output.sequence_tasks.clear(); for(int i = 0; i < input.sequence_tasks_size(); i++) { - kortex_driver::SequenceTask temp; + decltype(output.sequence_tasks)::value_type temp; ToRosData(input.sequence_tasks(i), temp); output.sequence_tasks.push_back(temp); } @@ -192,7 +235,7 @@ int ToRosData(Kinova::Api::Base::Sequence input, kortex_driver::Sequence &output output.tasks.clear(); for(int i = 0; i < input.tasks_size(); i++) { - kortex_driver::SequenceTask temp; + decltype(output.tasks)::value_type temp; ToRosData(input.tasks(i), temp); output.tasks.push_back(temp); } @@ -207,7 +250,7 @@ int ToRosData(Kinova::Api::Base::SequenceList input, kortex_driver::SequenceList output.sequence_list.clear(); for(int i = 0; i < input.sequence_list_size(); i++) { - kortex_driver::Sequence temp; + decltype(output.sequence_list)::value_type temp; ToRosData(input.sequence_list(i), temp); output.sequence_list.push_back(temp); } @@ -399,6 +442,14 @@ int ToRosData(Kinova::Api::Base::Action input, kortex_driver::Action &output) break; } + case Kinova::Api::Base::Action::kSendGpioCommand: + { + decltype(output.oneof_action_parameters.send_gpio_command)::value_type temp; + ToRosData(input.send_gpio_command(), temp); + output.oneof_action_parameters.send_gpio_command.push_back(temp); + break; + } + case Kinova::Api::Base::Action::kStopAction: { decltype(output.oneof_action_parameters.stop_action)::value_type temp; @@ -413,6 +464,22 @@ int ToRosData(Kinova::Api::Base::Action input, kortex_driver::Action &output) ToRosData(input.play_pre_computed_trajectory(), temp); output.oneof_action_parameters.play_pre_computed_trajectory.push_back(temp); break; + } + + case Kinova::Api::Base::Action::kExecuteSequence: + { + decltype(output.oneof_action_parameters.execute_sequence)::value_type temp; + ToRosData(input.execute_sequence(), temp); + output.oneof_action_parameters.execute_sequence.push_back(temp); + break; + } + + case Kinova::Api::Base::Action::kExecuteWaypointList: + { + decltype(output.oneof_action_parameters.execute_waypoint_list)::value_type temp; + ToRosData(input.execute_waypoint_list(), temp); + output.oneof_action_parameters.execute_waypoint_list.push_back(temp); + break; }} return 0; @@ -505,7 +572,7 @@ int ToRosData(Kinova::Api::Base::ActionList input, kortex_driver::ActionList &ou output.action_list.clear(); for(int i = 0; i < input.action_list_size(); i++) { - kortex_driver::Action temp; + decltype(output.action_list)::value_type temp; ToRosData(input.action_list(i), temp); output.action_list.push_back(temp); } @@ -605,7 +672,7 @@ int ToRosData(Kinova::Api::Base::WifiInformationList input, kortex_driver::WifiI output.wifi_information_list.clear(); for(int i = 0; i < input.wifi_information_list_size(); i++) { - kortex_driver::WifiInformation temp; + decltype(output.wifi_information_list)::value_type temp; ToRosData(input.wifi_information_list(i), temp); output.wifi_information_list.push_back(temp); } @@ -631,7 +698,7 @@ int ToRosData(Kinova::Api::Base::WifiConfigurationList input, kortex_driver::Wif output.wifi_configuration_list.clear(); for(int i = 0; i < input.wifi_configuration_list_size(); i++) { - kortex_driver::WifiConfiguration temp; + decltype(output.wifi_configuration_list)::value_type temp; ToRosData(input.wifi_configuration_list(i), temp); output.wifi_configuration_list.push_back(temp); } @@ -711,14 +778,14 @@ int ToRosData(Kinova::Api::Base::ProtectionZone input, kortex_driver::Protection output.limitations.clear(); for(int i = 0; i < input.limitations_size(); i++) { - kortex_driver::CartesianLimitation temp; + decltype(output.limitations)::value_type temp; ToRosData(input.limitations(i), temp); output.limitations.push_back(temp); } output.envelope_limitations.clear(); for(int i = 0; i < input.envelope_limitations_size(); i++) { - kortex_driver::CartesianLimitation temp; + decltype(output.envelope_limitations)::value_type temp; ToRosData(input.envelope_limitations(i), temp); output.envelope_limitations.push_back(temp); } @@ -733,7 +800,7 @@ int ToRosData(Kinova::Api::Base::ProtectionZoneList input, kortex_driver::Protec output.protection_zones.clear(); for(int i = 0; i < input.protection_zones_size(); i++) { - kortex_driver::ProtectionZone temp; + decltype(output.protection_zones)::value_type temp; ToRosData(input.protection_zones(i), temp); output.protection_zones.push_back(temp); } @@ -779,7 +846,7 @@ int ToRosData(Kinova::Api::Base::CartesianLimitationList input, kortex_driver::C output.limitations.clear(); for(int i = 0; i < input.limitations_size(); i++) { - kortex_driver::CartesianLimitation temp; + decltype(output.limitations)::value_type temp; ToRosData(input.limitations(i), temp); output.limitations.push_back(temp); } @@ -805,7 +872,7 @@ int ToRosData(Kinova::Api::Base::JointsLimitationsList input, kortex_driver::Joi output.joints_limitations.clear(); for(int i = 0; i < input.joints_limitations_size(); i++) { - kortex_driver::JointLimitation temp; + decltype(output.joints_limitations)::value_type temp; ToRosData(input.joints_limitations(i), temp); output.joints_limitations.push_back(temp); } @@ -951,7 +1018,7 @@ int ToRosData(Kinova::Api::Base::ControlModeInformation input, kortex_driver::Ba return 0; } -int ToRosData(Kinova::Api::Base::ControlModeNotification input, kortex_driver::ControlModeNotification &output) +int ToRosData(Kinova::Api::Base::ControlModeNotification input, kortex_driver::Base_ControlModeNotification &output) { output.control_mode = input.control_mode(); @@ -1137,7 +1204,7 @@ int ToRosData(Kinova::Api::Base::ControllerList input, kortex_driver::Controller output.handles.clear(); for(int i = 0; i < input.handles_size(); i++) { - kortex_driver::ControllerHandle temp; + decltype(output.handles)::value_type temp; ToRosData(input.handles(i), temp); output.handles.push_back(temp); } @@ -1176,6 +1243,24 @@ int ToRosData(Kinova::Api::Base::ActionNotification input, kortex_driver::Action ToRosData(input.user_handle(), output.user_handle); output.abort_details = input.abort_details(); ToRosData(input.connection(), output.connection); + output.trajectory_info.clear(); + for(int i = 0; i < input.trajectory_info_size(); i++) + { + decltype(output.trajectory_info)::value_type temp; + ToRosData(input.trajectory_info(i), temp); + output.trajectory_info.push_back(temp); + } + + + + return 0; +} +int ToRosData(Kinova::Api::Base::TrajectoryInfo input, kortex_driver::TrajectoryInfo &output) +{ + + output.trajectory_info_type = input.trajectory_info_type(); + output.waypoint_index = input.waypoint_index(); + output.joint_index = input.joint_index(); @@ -1234,7 +1319,7 @@ int ToRosData(Kinova::Api::Base::ConfigurationChangeNotificationList input, kort output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::ConfigurationChangeNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1249,7 +1334,7 @@ int ToRosData(Kinova::Api::Base::MappingInfoNotificationList input, kortex_drive output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::MappingInfoNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1264,7 +1349,7 @@ int ToRosData(Kinova::Api::Base::ControlModeNotificationList input, kortex_drive output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::ControlModeNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1279,7 +1364,7 @@ int ToRosData(Kinova::Api::Base::OperatingModeNotificationList input, kortex_dri output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::OperatingModeNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1294,7 +1379,7 @@ int ToRosData(Kinova::Api::Base::ServoingModeNotificationList input, kortex_driv output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::ServoingModeNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1309,7 +1394,7 @@ int ToRosData(Kinova::Api::Base::SequenceInfoNotificationList input, kortex_driv output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::SequenceInfoNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1324,7 +1409,7 @@ int ToRosData(Kinova::Api::Base::ProtectionZoneNotificationList input, kortex_dr output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::ProtectionZoneNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1339,7 +1424,7 @@ int ToRosData(Kinova::Api::Base::UserNotificationList input, kortex_driver::User output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::UserNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1354,7 +1439,7 @@ int ToRosData(Kinova::Api::Base::SafetyNotificationList input, kortex_driver::Sa output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::SafetyNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1369,7 +1454,7 @@ int ToRosData(Kinova::Api::Base::ControllerNotificationList input, kortex_driver output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::ControllerNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1384,7 +1469,7 @@ int ToRosData(Kinova::Api::Base::ActionNotificationList input, kortex_driver::Ac output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::ActionNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1399,7 +1484,7 @@ int ToRosData(Kinova::Api::Base::RobotEventNotificationList input, kortex_driver output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::RobotEventNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1414,7 +1499,7 @@ int ToRosData(Kinova::Api::Base::NetworkNotificationList input, kortex_driver::N output.notifications.clear(); for(int i = 0; i < input.notifications_size(); i++) { - kortex_driver::NetworkNotification temp; + decltype(output.notifications)::value_type temp; ToRosData(input.notifications(i), temp); output.notifications.push_back(temp); } @@ -1530,7 +1615,7 @@ int ToRosData(Kinova::Api::Base::Map input, kortex_driver::Map &output) output.elements.clear(); for(int i = 0; i < input.elements_size(); i++) { - kortex_driver::MapElement temp; + decltype(output.elements)::value_type temp; ToRosData(input.elements(i), temp); output.elements.push_back(temp); } @@ -1555,7 +1640,7 @@ int ToRosData(Kinova::Api::Base::MapList input, kortex_driver::MapList &output) output.map_list.clear(); for(int i = 0; i < input.map_list_size(); i++) { - kortex_driver::Map temp; + decltype(output.map_list)::value_type temp; ToRosData(input.map_list(i), temp); output.map_list.push_back(temp); } @@ -1584,14 +1669,14 @@ int ToRosData(Kinova::Api::Base::MapGroup input, kortex_driver::MapGroup &output output.children_map_group_handles.clear(); for(int i = 0; i < input.children_map_group_handles_size(); i++) { - kortex_driver::MapGroupHandle temp; + decltype(output.children_map_group_handles)::value_type temp; ToRosData(input.children_map_group_handles(i), temp); output.children_map_group_handles.push_back(temp); } output.map_handles.clear(); for(int i = 0; i < input.map_handles_size(); i++) { - kortex_driver::MapHandle temp; + decltype(output.map_handles)::value_type temp; ToRosData(input.map_handles(i), temp); output.map_handles.push_back(temp); } @@ -1607,7 +1692,7 @@ int ToRosData(Kinova::Api::Base::MapGroupList input, kortex_driver::MapGroupList output.map_groups.clear(); for(int i = 0; i < input.map_groups_size(); i++) { - kortex_driver::MapGroup temp; + decltype(output.map_groups)::value_type temp; ToRosData(input.map_groups(i), temp); output.map_groups.push_back(temp); } @@ -1626,7 +1711,7 @@ int ToRosData(Kinova::Api::Base::Mapping input, kortex_driver::Mapping &output) output.map_group_handles.clear(); for(int i = 0; i < input.map_group_handles_size(); i++) { - kortex_driver::MapGroupHandle temp; + decltype(output.map_group_handles)::value_type temp; ToRosData(input.map_group_handles(i), temp); output.map_group_handles.push_back(temp); } @@ -1634,7 +1719,7 @@ int ToRosData(Kinova::Api::Base::Mapping input, kortex_driver::Mapping &output) output.map_handles.clear(); for(int i = 0; i < input.map_handles_size(); i++) { - kortex_driver::MapHandle temp; + decltype(output.map_handles)::value_type temp; ToRosData(input.map_handles(i), temp); output.map_handles.push_back(temp); } @@ -1650,7 +1735,7 @@ int ToRosData(Kinova::Api::Base::MappingList input, kortex_driver::MappingList & output.mappings.clear(); for(int i = 0; i < input.mappings_size(); i++) { - kortex_driver::Mapping temp; + decltype(output.mappings)::value_type temp; ToRosData(input.mappings(i), temp); output.mappings.push_back(temp); } @@ -1880,7 +1965,7 @@ int ToRosData(Kinova::Api::Base::JointAngles input, kortex_driver::JointAngles & output.joint_angles.clear(); for(int i = 0; i < input.joint_angles_size(); i++) { - kortex_driver::JointAngle temp; + decltype(output.joint_angles)::value_type temp; ToRosData(input.joint_angles(i), temp); output.joint_angles.push_back(temp); } @@ -1905,7 +1990,7 @@ int ToRosData(Kinova::Api::Base::JointSpeeds input, kortex_driver::Base_JointSpe output.joint_speeds.clear(); for(int i = 0; i < input.joint_speeds_size(); i++) { - kortex_driver::JointSpeed temp; + decltype(output.joint_speeds)::value_type temp; ToRosData(input.joint_speeds(i), temp); output.joint_speeds.push_back(temp); } @@ -1932,7 +2017,7 @@ int ToRosData(Kinova::Api::Base::JointTorques input, kortex_driver::JointTorques output.joint_torques.clear(); for(int i = 0; i < input.joint_torques_size(); i++) { - kortex_driver::JointTorque temp; + decltype(output.joint_torques)::value_type temp; ToRosData(input.joint_torques(i), temp); output.joint_torques.push_back(temp); } @@ -1979,7 +2064,7 @@ int ToRosData(Kinova::Api::Base::Gripper input, kortex_driver::Gripper &output) output.finger.clear(); for(int i = 0; i < input.finger_size(); i++) { - kortex_driver::Finger temp; + decltype(output.finger)::value_type temp; ToRosData(input.finger(i), temp); output.finger.push_back(temp); } @@ -1996,6 +2081,18 @@ int ToRosData(Kinova::Api::Base::Finger input, kortex_driver::Finger &output) + return 0; +} +int ToRosData(Kinova::Api::Base::GpioCommand input, kortex_driver::GpioCommand &output) +{ + + output.port_identifier = input.port_identifier(); + output.pin_identifier = input.pin_identifier(); + output.action = input.action(); + output.period = input.period(); + + + return 0; } int ToRosData(Kinova::Api::Base::SystemTime input, kortex_driver::SystemTime &output) @@ -2040,7 +2137,7 @@ int ToRosData(Kinova::Api::Base::ControllerConfigurationList input, kortex_drive output.controller_configurations.clear(); for(int i = 0; i < input.controller_configurations_size(); i++) { - kortex_driver::ControllerConfiguration temp; + decltype(output.controller_configurations)::value_type temp; ToRosData(input.controller_configurations(i), temp); output.controller_configurations.push_back(temp); } @@ -2099,7 +2196,7 @@ int ToRosData(Kinova::Api::Base::BridgeList input, kortex_driver::BridgeList &ou output.bridgeConfig.clear(); for(int i = 0; i < input.bridgeconfig_size(); i++) { - kortex_driver::BridgeConfig temp; + decltype(output.bridgeConfig)::value_type temp; ToRosData(input.bridgeconfig(i), temp); output.bridgeConfig.push_back(temp); } @@ -2156,7 +2253,7 @@ int ToRosData(Kinova::Api::Base::PreComputedJointTrajectory input, kortex_driver output.trajectory_elements.clear(); for(int i = 0; i < input.trajectory_elements_size(); i++) { - kortex_driver::PreComputedJointTrajectoryElement temp; + decltype(output.trajectory_elements)::value_type temp; ToRosData(input.trajectory_elements(i), temp); output.trajectory_elements.push_back(temp); } @@ -2199,6 +2296,7 @@ int ToRosData(Kinova::Api::Base::TrajectoryErrorElement input, kortex_driver::Tr output.max_value = input.max_value(); output.index = input.index(); output.message = input.message(); + output.waypoint_index = input.waypoint_index(); @@ -2210,13 +2308,115 @@ int ToRosData(Kinova::Api::Base::TrajectoryErrorReport input, kortex_driver::Tra output.trajectory_error_elements.clear(); for(int i = 0; i < input.trajectory_error_elements_size(); i++) { - kortex_driver::TrajectoryErrorElement temp; + decltype(output.trajectory_error_elements)::value_type temp; ToRosData(input.trajectory_error_elements(i), temp); output.trajectory_error_elements.push_back(temp); } + return 0; +} +int ToRosData(Kinova::Api::Base::WaypointValidationReport input, kortex_driver::WaypointValidationReport &output) +{ + + ToRosData(input.trajectory_error_report(), output.trajectory_error_report); + ToRosData(input.optimal_waypoint_list(), output.optimal_waypoint_list); + + + + return 0; +} +int ToRosData(Kinova::Api::Base::Waypoint input, kortex_driver::Waypoint &output) +{ + + output.name = input.name(); + + + auto oneof_type_type_of_waypoint = input.type_of_waypoint_case(); + switch(oneof_type_type_of_waypoint) + { + + case Kinova::Api::Base::Waypoint::kAngularWaypoint: + { + decltype(output.oneof_type_of_waypoint.angular_waypoint)::value_type temp; + ToRosData(input.angular_waypoint(), temp); + output.oneof_type_of_waypoint.angular_waypoint.push_back(temp); + break; + } + + case Kinova::Api::Base::Waypoint::kCartesianWaypoint: + { + decltype(output.oneof_type_of_waypoint.cartesian_waypoint)::value_type temp; + ToRosData(input.cartesian_waypoint(), temp); + output.oneof_type_of_waypoint.cartesian_waypoint.push_back(temp); + break; + }} + + return 0; +} +int ToRosData(Kinova::Api::Base::AngularWaypoint input, kortex_driver::AngularWaypoint &output) +{ + + output.angles.clear(); + for(int i = 0; i < input.angles_size(); i++) + { + output.angles.push_back(input.angles(i)); + } + output.maximum_velocities.clear(); + for(int i = 0; i < input.maximum_velocities_size(); i++) + { + output.maximum_velocities.push_back(input.maximum_velocities(i)); + } + output.duration = input.duration(); + + + + return 0; +} +int ToRosData(Kinova::Api::Base::CartesianWaypoint input, kortex_driver::CartesianWaypoint &output) +{ + + ToRosData(input.pose(), output.pose); + output.reference_frame = input.reference_frame(); + output.maximum_linear_velocity = input.maximum_linear_velocity(); + output.maximum_angular_velocity = input.maximum_angular_velocity(); + output.blending_radius = input.blending_radius(); + + + + return 0; +} +int ToRosData(Kinova::Api::Base::WaypointList input, kortex_driver::WaypointList &output) +{ + + output.waypoints.clear(); + for(int i = 0; i < input.waypoints_size(); i++) + { + decltype(output.waypoints)::value_type temp; + ToRosData(input.waypoints(i), temp); + output.waypoints.push_back(temp); + } + output.duration = input.duration(); + output.use_optimal_blending = input.use_optimal_blending(); + + + + return 0; +} +int ToRosData(Kinova::Api::Base::KinematicTrajectoryConstraints input, kortex_driver::KinematicTrajectoryConstraints &output) +{ + + output.angular_velocities.clear(); + for(int i = 0; i < input.angular_velocities_size(); i++) + { + output.angular_velocities.push_back(input.angular_velocities(i)); + } + output.linear_velocity = input.linear_velocity(); + output.angular_velocity = input.angular_velocity(); + + + return 0; } int ToRosData(Kinova::Api::Base::FirmwareBundleVersions input, kortex_driver::FirmwareBundleVersions &output) @@ -2226,7 +2426,7 @@ int ToRosData(Kinova::Api::Base::FirmwareBundleVersions input, kortex_driver::Fi output.components_versions.clear(); for(int i = 0; i < input.components_versions_size(); i++) { - kortex_driver::FirmwareComponentVersion temp; + decltype(output.components_versions)::value_type temp; ToRosData(input.components_versions(i), temp); output.components_versions.push_back(temp); } @@ -2244,5 +2444,15 @@ int ToRosData(Kinova::Api::Base::FirmwareComponentVersion input, kortex_driver:: + return 0; +} +int ToRosData(Kinova::Api::Base::IKData input, kortex_driver::IKData &output) +{ + + ToRosData(input.cartesian_pose(), output.cartesian_pose); + ToRosData(input.guess(), output.guess); + + + return 0; } diff --git a/kortex_driver/src/generated/robot/base_services.cpp b/kortex_driver/src/generated/robot/base_services.cpp index 1a8a6dd9..6ce347fd 100644 --- a/kortex_driver/src/generated/robot/base_services.cpp +++ b/kortex_driver/src/generated/robot/base_services.cpp @@ -54,7 +54,7 @@ BaseRobotServices::BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api:: m_is_activated_ConfigurationChangeTopic = false; m_pub_MappingInfoTopic = m_node_handle.advertise("mapping_info_topic", 1000); m_is_activated_MappingInfoTopic = false; - m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); m_is_activated_ControlModeTopic = false; m_pub_OperatingModeTopic = m_node_handle.advertise("operating_mode_topic", 1000); m_is_activated_OperatingModeTopic = false; @@ -140,7 +140,7 @@ BaseRobotServices::BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api:: m_serviceBase_Unsubscribe = m_node_handle.advertiseService("base/unsubscribe", &BaseRobotServices::Base_Unsubscribe, this); m_serviceOnNotificationConfigurationChangeTopic = m_node_handle.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseRobotServices::OnNotificationConfigurationChangeTopic, this); m_serviceOnNotificationMappingInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseRobotServices::OnNotificationMappingInfoTopic, this); - m_serviceOnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseRobotServices::OnNotificationControlModeTopic, this); + m_serviceBase_OnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseRobotServices::Base_OnNotificationControlModeTopic, this); m_serviceOnNotificationOperatingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseRobotServices::OnNotificationOperatingModeTopic, this); m_serviceOnNotificationSequenceInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseRobotServices::OnNotificationSequenceInfoTopic, this); m_serviceOnNotificationProtectionZoneTopic = m_node_handle.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseRobotServices::OnNotificationProtectionZoneTopic, this); @@ -220,12 +220,16 @@ BaseRobotServices::BaseRobotServices(ros::NodeHandle& node_handle, Kinova::Api:: m_serviceDeleteAllSequenceTasks = m_node_handle.advertiseService("base/delete_all_sequence_tasks", &BaseRobotServices::DeleteAllSequenceTasks, this); m_serviceTakeSnapshot = m_node_handle.advertiseService("base/take_snapshot", &BaseRobotServices::TakeSnapshot, this); m_serviceGetFirmwareBundleVersions = m_node_handle.advertiseService("base/get_firmware_bundle_versions", &BaseRobotServices::GetFirmwareBundleVersions, this); + m_serviceExecuteWaypointTrajectory = m_node_handle.advertiseService("base/execute_waypoint_trajectory", &BaseRobotServices::ExecuteWaypointTrajectory, this); m_serviceMoveSequenceTask = m_node_handle.advertiseService("base/move_sequence_task", &BaseRobotServices::MoveSequenceTask, this); m_serviceDuplicateMapping = m_node_handle.advertiseService("base/duplicate_mapping", &BaseRobotServices::DuplicateMapping, this); m_serviceDuplicateMap = m_node_handle.advertiseService("base/duplicate_map", &BaseRobotServices::DuplicateMap, this); m_serviceSetControllerConfiguration = m_node_handle.advertiseService("base/set_controller_configuration", &BaseRobotServices::SetControllerConfiguration, this); m_serviceGetControllerConfiguration = m_node_handle.advertiseService("base/get_controller_configuration", &BaseRobotServices::GetControllerConfiguration, this); m_serviceGetAllControllerConfigurations = m_node_handle.advertiseService("base/get_all_controller_configurations", &BaseRobotServices::GetAllControllerConfigurations, this); + m_serviceComputeForwardKinematics = m_node_handle.advertiseService("base/compute_forward_kinematics", &BaseRobotServices::ComputeForwardKinematics, this); + m_serviceComputeInverseKinematics = m_node_handle.advertiseService("base/compute_inverse_kinematics", &BaseRobotServices::ComputeInverseKinematics, this); + m_serviceValidateWaypointList = m_node_handle.advertiseService("base/validate_waypoint_list", &BaseRobotServices::ValidateWaypointList, this); } bool BaseRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) @@ -2172,8 +2176,9 @@ void BaseRobotServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotifi m_pub_MappingInfoTopic.publish(ros_msg); } -bool BaseRobotServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) +bool BaseRobotServices::Base_OnNotificationControlModeTopic(kortex_driver::Base_OnNotificationControlModeTopic::Request &req, kortex_driver::Base_OnNotificationControlModeTopic::Response &res) { + ROS_WARN("The base/activate_publishing_of_control_mode_topic service is now deprecated and will be removed in a future release."); // If the notification is already activated, don't activate multiple times if (m_is_activated_ControlModeTopic) @@ -2213,7 +2218,7 @@ bool BaseRobotServices::OnNotificationControlModeTopic(kortex_driver::OnNotifica } void BaseRobotServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) { - kortex_driver::ControlModeNotification ros_msg; + kortex_driver::Base_ControlModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControlModeTopic.publish(ros_msg); } @@ -2542,6 +2547,7 @@ void BaseRobotServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotifica bool BaseRobotServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) { + ROS_WARN("The base/play_cartesian_trajectory service is now deprecated and will be removed in a future release."); Kinova::Api::Base::ConstrainedPose input; ToProtoData(req.input, &input); @@ -2574,6 +2580,7 @@ bool BaseRobotServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTraj bool BaseRobotServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) { + ROS_WARN("The base/play_cartesian_trajectory_position service is now deprecated and will be removed in a future release."); Kinova::Api::Base::ConstrainedPosition input; ToProtoData(req.input, &input); @@ -2606,6 +2613,7 @@ bool BaseRobotServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCarte bool BaseRobotServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) { + ROS_WARN("The base/play_cartesian_trajectory_orientation service is now deprecated and will be removed in a future release."); Kinova::Api::Base::ConstrainedOrientation input; ToProtoData(req.input, &input); @@ -2829,6 +2837,7 @@ bool BaseRobotServices::SendTwistCommand(kortex_driver::SendTwistCommand::Reques bool BaseRobotServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) { + ROS_WARN("The base/play_joint_trajectory service is now deprecated and will be removed in a future release."); Kinova::Api::Base::ConstrainedJointAngles input; ToProtoData(req.input, &input); @@ -2861,6 +2870,7 @@ bool BaseRobotServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory:: bool BaseRobotServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) { + ROS_WARN("The base/play_selected_joint_trajectory service is now deprecated and will be removed in a future release."); Kinova::Api::Base::ConstrainedJointAngle input; ToProtoData(req.input, &input); @@ -3181,6 +3191,7 @@ bool BaseRobotServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::Reques bool BaseRobotServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) { + ROS_WARN("The base/get_control_mode service is now deprecated and will be removed in a future release."); Kinova::Api::Base::ControlModeInformation output; @@ -4947,6 +4958,38 @@ bool BaseRobotServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwareBund return true; } +bool BaseRobotServices::ExecuteWaypointTrajectory(kortex_driver::ExecuteWaypointTrajectory::Request &req, kortex_driver::ExecuteWaypointTrajectory::Response &res) +{ + + Kinova::Api::Base::WaypointList input; + ToProtoData(req.input, &input); + kortex_driver::KortexError result_error; + + try + { + m_base->ExecuteWaypointTrajectory(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + return true; +} + bool BaseRobotServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) { @@ -5148,3 +5191,108 @@ bool BaseRobotServices::GetAllControllerConfigurations(kortex_driver::GetAllCont ToRosData(output, res.output); return true; } + +bool BaseRobotServices::ComputeForwardKinematics(kortex_driver::ComputeForwardKinematics::Request &req, kortex_driver::ComputeForwardKinematics::Response &res) +{ + + Kinova::Api::Base::JointAngles input; + ToProtoData(req.input, &input); + Kinova::Api::Base::Pose output; + + kortex_driver::KortexError result_error; + + try + { + output = m_base->ComputeForwardKinematics(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseRobotServices::ComputeInverseKinematics(kortex_driver::ComputeInverseKinematics::Request &req, kortex_driver::ComputeInverseKinematics::Response &res) +{ + + Kinova::Api::Base::IKData input; + ToProtoData(req.input, &input); + Kinova::Api::Base::JointAngles output; + + kortex_driver::KortexError result_error; + + try + { + output = m_base->ComputeInverseKinematics(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} + +bool BaseRobotServices::ValidateWaypointList(kortex_driver::ValidateWaypointList::Request &req, kortex_driver::ValidateWaypointList::Response &res) +{ + + Kinova::Api::Base::WaypointList input; + ToProtoData(req.input, &input); + Kinova::Api::Base::WaypointValidationReport output; + + kortex_driver::KortexError result_error; + + try + { + output = m_base->ValidateWaypointList(input, m_current_device_id, m_api_options); + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} diff --git a/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp index e064758e..98c164eb 100644 --- a/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/basecyclic_ros_converter.cpp @@ -140,7 +140,7 @@ int ToRosData(Kinova::Api::BaseCyclic::CustomData input, kortex_driver::BaseCycl output.actuators_custom_data.clear(); for(int i = 0; i < input.actuators_custom_data_size(); i++) { - kortex_driver::ActuatorCustomData temp; + decltype(output.actuators_custom_data)::value_type temp; ToRosData(input.actuators_custom_data(i), temp); output.actuators_custom_data.push_back(temp); } @@ -157,7 +157,7 @@ int ToRosData(Kinova::Api::BaseCyclic::Command input, kortex_driver::BaseCyclic_ output.actuators.clear(); for(int i = 0; i < input.actuators_size(); i++) { - kortex_driver::ActuatorCommand temp; + decltype(output.actuators)::value_type temp; ToRosData(input.actuators(i), temp); output.actuators.push_back(temp); } @@ -175,7 +175,7 @@ int ToRosData(Kinova::Api::BaseCyclic::Feedback input, kortex_driver::BaseCyclic output.actuators.clear(); for(int i = 0; i < input.actuators_size(); i++) { - kortex_driver::ActuatorFeedback temp; + decltype(output.actuators)::value_type temp; ToRosData(input.actuators(i), temp); output.actuators.push_back(temp); } diff --git a/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp index 844f103d..02bba246 100644 --- a/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_proto_converter.cpp @@ -195,3 +195,13 @@ int ToProtoData(kortex_driver::ControlConfig_ControlModeInformation input, Kinov return 0; } +int ToProtoData(kortex_driver::ControlConfig_ControlModeNotification input, Kinova::Api::ControlConfig::ControlModeNotification *output) +{ + + output->set_control_mode((Kinova::Api::ControlConfig::ControlMode)input.control_mode); + ToProtoData(input.timestamp, output->mutable_timestamp()); + ToProtoData(input.user_handle, output->mutable_user_handle()); + ToProtoData(input.connection, output->mutable_connection()); + + return 0; +} diff --git a/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp index 5032d465..f83d7ec5 100644 --- a/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_ros_converter.cpp @@ -169,7 +169,7 @@ int ToRosData(Kinova::Api::ControlConfig::KinematicLimitsList input, kortex_driv output.kinematic_limits_list.clear(); for(int i = 0; i < input.kinematic_limits_list_size(); i++) { - kortex_driver::KinematicLimits temp; + decltype(output.kinematic_limits_list)::value_type temp; ToRosData(input.kinematic_limits_list(i), temp); output.kinematic_limits_list.push_back(temp); } @@ -231,5 +231,17 @@ int ToRosData(Kinova::Api::ControlConfig::ControlModeInformation input, kortex_d + return 0; +} +int ToRosData(Kinova::Api::ControlConfig::ControlModeNotification input, kortex_driver::ControlConfig_ControlModeNotification &output) +{ + + output.control_mode = input.control_mode(); + ToRosData(input.timestamp(), output.timestamp); + ToRosData(input.user_handle(), output.user_handle); + ToRosData(input.connection(), output.connection); + + + return 0; } diff --git a/kortex_driver/src/generated/robot/controlconfig_services.cpp b/kortex_driver/src/generated/robot/controlconfig_services.cpp index 0e75bf8f..469e8d42 100644 --- a/kortex_driver/src/generated/robot/controlconfig_services.cpp +++ b/kortex_driver/src/generated/robot/controlconfig_services.cpp @@ -52,6 +52,8 @@ ControlConfigRobotServices::ControlConfigRobotServices(ros::NodeHandle& node_han m_pub_Error = m_node_handle.advertise("kortex_error", 1000); m_pub_ControlConfigurationTopic = m_node_handle.advertise("control_configuration_topic", 1000); m_is_activated_ControlConfigurationTopic = false; + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); + m_is_activated_ControlModeTopic = false; m_serviceSetDeviceID = m_node_handle.advertiseService("control_config/set_device_id", &ControlConfigRobotServices::SetDeviceID, this); m_serviceSetApiOptions = m_node_handle.advertiseService("control_config/set_api_options", &ControlConfigRobotServices::SetApiOptions, this); @@ -85,6 +87,7 @@ ControlConfigRobotServices::ControlConfigRobotServices(ros::NodeHandle& node_han m_serviceResetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigRobotServices::ResetTwistLinearSoftLimit, this); m_serviceResetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigRobotServices::ResetTwistAngularSoftLimit, this); m_serviceResetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigRobotServices::ResetJointAccelerationSoftLimits, this); + m_serviceControlConfig_OnNotificationControlModeTopic = m_node_handle.advertiseService("control_config/activate_publishing_of_control_mode_topic", &ControlConfigRobotServices::ControlConfig_OnNotificationControlModeTopic, this); } bool ControlConfigRobotServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) @@ -1069,3 +1072,49 @@ bool ControlConfigRobotServices::ResetJointAccelerationSoftLimits(kortex_driver: ToRosData(output, res.output); return true; } + +bool ControlConfigRobotServices::ControlConfig_OnNotificationControlModeTopic(kortex_driver::ControlConfig_OnNotificationControlModeTopic::Request &req, kortex_driver::ControlConfig_OnNotificationControlModeTopic::Response &res) +{ + + // If the notification is already activated, don't activate multiple times + if (m_is_activated_ControlModeTopic) + return true; + Kinova::Api::Common::NotificationOptions input; + ToProtoData(req.input, &input); + Kinova::Api::Common::NotificationHandle output; + + kortex_driver::KortexError result_error; + + try + { + std::function< void (Kinova::Api::ControlConfig::ControlModeNotification) > callback = std::bind(&ControlConfigRobotServices::cb_ControlModeTopic, this, std::placeholders::_1); + output = m_controlconfig->OnNotificationControlModeTopic(callback, input, m_current_device_id); + m_is_activated_ControlModeTopic = true; + } + + catch (Kinova::Api::KDetailedException& ex) + { + result_error.subCode = ex.getErrorInfo().getError().error_sub_code(); + result_error.code = ex.getErrorInfo().getError().error_code(); + result_error.description = ex.toString(); + m_pub_Error.publish(result_error); + ROS_INFO("Kortex exception"); + ROS_INFO("KINOVA exception error code: %d\n", ex.getErrorInfo().getError().error_code()); + ROS_INFO("KINOVA exception error sub code: %d\n", ex.getErrorInfo().getError().error_sub_code()); + ROS_INFO("KINOVA exception description: %s\n", ex.what()); + return false; + } + catch (std::runtime_error& ex2) + { + ROS_INFO("%s", ex2.what()); + return false; + } + ToRosData(output, res.output); + return true; +} +void ControlConfigRobotServices::cb_ControlModeTopic(Kinova::Api::ControlConfig::ControlModeNotification notif) +{ + kortex_driver::ControlConfig_ControlModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlModeTopic.publish(ros_msg); +} diff --git a/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp index 15e0ef16..90b090af 100644 --- a/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/deviceconfig_ros_converter.cpp @@ -151,7 +151,7 @@ int ToRosData(Kinova::Api::DeviceConfig::SafetyInformationList input, kortex_dri output.information.clear(); for(int i = 0; i < input.information_size(); i++) { - kortex_driver::SafetyInformation temp; + decltype(output.information)::value_type temp; ToRosData(input.information(i), temp); output.information.push_back(temp); } @@ -198,7 +198,7 @@ int ToRosData(Kinova::Api::DeviceConfig::SafetyConfigurationList input, kortex_d output.configuration.clear(); for(int i = 0; i < input.configuration_size(); i++) { - kortex_driver::SafetyConfiguration temp; + decltype(output.configuration)::value_type temp; ToRosData(input.configuration(i), temp); output.configuration.push_back(temp); } @@ -250,7 +250,7 @@ int ToRosData(Kinova::Api::DeviceConfig::Calibration input, kortex_driver::Calib output.calibration_parameter.clear(); for(int i = 0; i < input.calibration_parameter_size(); i++) { - kortex_driver::CalibrationParameter temp; + decltype(output.calibration_parameter)::value_type temp; ToRosData(input.calibration_parameter(i), temp); output.calibration_parameter.push_back(temp); } diff --git a/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp b/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp index 3b5e95f6..705f53db 100644 --- a/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/devicemanager_ros_converter.cpp @@ -22,7 +22,7 @@ int ToRosData(Kinova::Api::DeviceManager::DeviceHandles input, kortex_driver::De output.device_handle.clear(); for(int i = 0; i < input.device_handle_size(); i++) { - kortex_driver::DeviceHandle temp; + decltype(output.device_handle)::value_type temp; ToRosData(input.device_handle(i), temp); output.device_handle.push_back(temp); } diff --git a/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp b/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp index 09523044..bcd7bd8a 100644 --- a/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/grippercyclic_ros_converter.cpp @@ -45,7 +45,7 @@ int ToRosData(Kinova::Api::GripperCyclic::Command input, kortex_driver::GripperC output.motor_cmd.clear(); for(int i = 0; i < input.motor_cmd_size(); i++) { - kortex_driver::MotorCommand temp; + decltype(output.motor_cmd)::value_type temp; ToRosData(input.motor_cmd(i), temp); output.motor_cmd.push_back(temp); } @@ -80,7 +80,7 @@ int ToRosData(Kinova::Api::GripperCyclic::Feedback input, kortex_driver::Gripper output.motor.clear(); for(int i = 0; i < input.motor_size(); i++) { - kortex_driver::MotorFeedback temp; + decltype(output.motor)::value_type temp; ToRosData(input.motor(i), temp); output.motor.push_back(temp); } @@ -121,7 +121,7 @@ int ToRosData(Kinova::Api::GripperCyclic::CustomData input, kortex_driver::Gripp output.motor_custom_data.clear(); for(int i = 0; i < input.motor_custom_data_size(); i++) { - kortex_driver::CustomDataUnit temp; + decltype(output.motor_custom_data)::value_type temp; ToRosData(input.motor_custom_data(i), temp); output.motor_custom_data.push_back(temp); } diff --git a/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp b/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp index 1c0bc732..07474f6d 100644 --- a/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_proto_converter.cpp @@ -40,7 +40,7 @@ int ToProtoData(kortex_driver::GPIOIdentification input, Kinova::Api::Interconne return 0; } -int ToProtoData(kortex_driver::GPIOConfiguration input, Kinova::Api::InterconnectConfig::GPIOConfiguration *output) +int ToProtoData(kortex_driver::InterconnectConfig_GPIOConfiguration input, Kinova::Api::InterconnectConfig::GPIOConfiguration *output) { output->set_identifier((Kinova::Api::InterconnectConfig::GPIOIdentifier)input.identifier); diff --git a/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp b/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp index bc2f660b..e263f425 100644 --- a/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp +++ b/kortex_driver/src/generated/robot/interconnectconfig_ros_converter.cpp @@ -46,7 +46,7 @@ int ToRosData(Kinova::Api::InterconnectConfig::GPIOIdentification input, kortex_ return 0; } -int ToRosData(Kinova::Api::InterconnectConfig::GPIOConfiguration input, kortex_driver::GPIOConfiguration &output) +int ToRosData(Kinova::Api::InterconnectConfig::GPIOConfiguration input, kortex_driver::InterconnectConfig_GPIOConfiguration &output) { output.identifier = input.identifier(); diff --git a/kortex_driver/src/generated/simulation/base_services.cpp b/kortex_driver/src/generated/simulation/base_services.cpp index a8e3589f..d3c3c5b8 100644 --- a/kortex_driver/src/generated/simulation/base_services.cpp +++ b/kortex_driver/src/generated/simulation/base_services.cpp @@ -50,7 +50,7 @@ BaseSimulationServices::BaseSimulationServices(ros::NodeHandle& node_handle): m_is_activated_ConfigurationChangeTopic = false; m_pub_MappingInfoTopic = m_node_handle.advertise("mapping_info_topic", 1000); m_is_activated_MappingInfoTopic = false; - m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); m_is_activated_ControlModeTopic = false; m_pub_OperatingModeTopic = m_node_handle.advertise("operating_mode_topic", 1000); m_is_activated_OperatingModeTopic = false; @@ -136,7 +136,7 @@ BaseSimulationServices::BaseSimulationServices(ros::NodeHandle& node_handle): m_serviceBase_Unsubscribe = m_node_handle.advertiseService("base/unsubscribe", &BaseSimulationServices::Base_Unsubscribe, this); m_serviceOnNotificationConfigurationChangeTopic = m_node_handle.advertiseService("base/activate_publishing_of_configuration_change_topic", &BaseSimulationServices::OnNotificationConfigurationChangeTopic, this); m_serviceOnNotificationMappingInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_mapping_info_topic", &BaseSimulationServices::OnNotificationMappingInfoTopic, this); - m_serviceOnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseSimulationServices::OnNotificationControlModeTopic, this); + m_serviceBase_OnNotificationControlModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_control_mode_topic", &BaseSimulationServices::Base_OnNotificationControlModeTopic, this); m_serviceOnNotificationOperatingModeTopic = m_node_handle.advertiseService("base/activate_publishing_of_operating_mode_topic", &BaseSimulationServices::OnNotificationOperatingModeTopic, this); m_serviceOnNotificationSequenceInfoTopic = m_node_handle.advertiseService("base/activate_publishing_of_sequence_info_topic", &BaseSimulationServices::OnNotificationSequenceInfoTopic, this); m_serviceOnNotificationProtectionZoneTopic = m_node_handle.advertiseService("base/activate_publishing_of_protection_zone_topic", &BaseSimulationServices::OnNotificationProtectionZoneTopic, this); @@ -216,12 +216,16 @@ BaseSimulationServices::BaseSimulationServices(ros::NodeHandle& node_handle): m_serviceDeleteAllSequenceTasks = m_node_handle.advertiseService("base/delete_all_sequence_tasks", &BaseSimulationServices::DeleteAllSequenceTasks, this); m_serviceTakeSnapshot = m_node_handle.advertiseService("base/take_snapshot", &BaseSimulationServices::TakeSnapshot, this); m_serviceGetFirmwareBundleVersions = m_node_handle.advertiseService("base/get_firmware_bundle_versions", &BaseSimulationServices::GetFirmwareBundleVersions, this); + m_serviceExecuteWaypointTrajectory = m_node_handle.advertiseService("base/execute_waypoint_trajectory", &BaseSimulationServices::ExecuteWaypointTrajectory, this); m_serviceMoveSequenceTask = m_node_handle.advertiseService("base/move_sequence_task", &BaseSimulationServices::MoveSequenceTask, this); m_serviceDuplicateMapping = m_node_handle.advertiseService("base/duplicate_mapping", &BaseSimulationServices::DuplicateMapping, this); m_serviceDuplicateMap = m_node_handle.advertiseService("base/duplicate_map", &BaseSimulationServices::DuplicateMap, this); m_serviceSetControllerConfiguration = m_node_handle.advertiseService("base/set_controller_configuration", &BaseSimulationServices::SetControllerConfiguration, this); m_serviceGetControllerConfiguration = m_node_handle.advertiseService("base/get_controller_configuration", &BaseSimulationServices::GetControllerConfiguration, this); m_serviceGetAllControllerConfigurations = m_node_handle.advertiseService("base/get_all_controller_configurations", &BaseSimulationServices::GetAllControllerConfigurations, this); + m_serviceComputeForwardKinematics = m_node_handle.advertiseService("base/compute_forward_kinematics", &BaseSimulationServices::ComputeForwardKinematics, this); + m_serviceComputeInverseKinematics = m_node_handle.advertiseService("base/compute_inverse_kinematics", &BaseSimulationServices::ComputeInverseKinematics, this); + m_serviceValidateWaypointList = m_node_handle.advertiseService("base/validate_waypoint_list", &BaseSimulationServices::ValidateWaypointList, this); } bool BaseSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) @@ -1121,14 +1125,15 @@ void BaseSimulationServices::cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoN m_pub_MappingInfoTopic.publish(ros_msg); } -bool BaseSimulationServices::OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) +bool BaseSimulationServices::Base_OnNotificationControlModeTopic(kortex_driver::Base_OnNotificationControlModeTopic::Request &req, kortex_driver::Base_OnNotificationControlModeTopic::Response &res) { + ROS_WARN("The base/activate_publishing_of_control_mode_topic service is now deprecated and will be removed in a future release."); m_is_activated_ControlModeTopic = true; - if (OnNotificationControlModeTopicHandler) + if (Base_OnNotificationControlModeTopicHandler) { - res = OnNotificationControlModeTopicHandler(req); + res = Base_OnNotificationControlModeTopicHandler(req); } else { @@ -1138,7 +1143,7 @@ bool BaseSimulationServices::OnNotificationControlModeTopic(kortex_driver::OnNot } void BaseSimulationServices::cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) { - kortex_driver::ControlModeNotification ros_msg; + kortex_driver::Base_ControlModeNotification ros_msg; ToRosData(notif, ros_msg); m_pub_ControlModeTopic.publish(ros_msg); } @@ -1299,6 +1304,7 @@ void BaseSimulationServices::cb_RobotEventTopic(Kinova::Api::Base::RobotEventNot bool BaseSimulationServices::PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) { + ROS_WARN("The base/play_cartesian_trajectory service is now deprecated and will be removed in a future release."); if (PlayCartesianTrajectoryHandler) @@ -1314,6 +1320,7 @@ bool BaseSimulationServices::PlayCartesianTrajectory(kortex_driver::PlayCartesia bool BaseSimulationServices::PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) { + ROS_WARN("The base/play_cartesian_trajectory_position service is now deprecated and will be removed in a future release."); if (PlayCartesianTrajectoryPositionHandler) @@ -1329,6 +1336,7 @@ bool BaseSimulationServices::PlayCartesianTrajectoryPosition(kortex_driver::Play bool BaseSimulationServices::PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) { + ROS_WARN("The base/play_cartesian_trajectory_orientation service is now deprecated and will be removed in a future release."); if (PlayCartesianTrajectoryOrientationHandler) @@ -1434,6 +1442,7 @@ bool BaseSimulationServices::SendTwistCommand(kortex_driver::SendTwistCommand::R bool BaseSimulationServices::PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) { + ROS_WARN("The base/play_joint_trajectory service is now deprecated and will be removed in a future release."); if (PlayJointTrajectoryHandler) @@ -1449,6 +1458,7 @@ bool BaseSimulationServices::PlayJointTrajectory(kortex_driver::PlayJointTraject bool BaseSimulationServices::PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) { + ROS_WARN("The base/play_selected_joint_trajectory service is now deprecated and will be removed in a future release."); if (PlaySelectedJointTrajectoryHandler) @@ -1599,6 +1609,7 @@ bool BaseSimulationServices::Base_ClearFaults(kortex_driver::Base_ClearFaults::R bool BaseSimulationServices::Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) { + ROS_WARN("The base/get_control_mode service is now deprecated and will be removed in a future release."); if (Base_GetControlModeHandler) @@ -2413,6 +2424,21 @@ bool BaseSimulationServices::GetFirmwareBundleVersions(kortex_driver::GetFirmwar return true; } +bool BaseSimulationServices::ExecuteWaypointTrajectory(kortex_driver::ExecuteWaypointTrajectory::Request &req, kortex_driver::ExecuteWaypointTrajectory::Response &res) +{ + + + if (ExecuteWaypointTrajectoryHandler) + { + res = ExecuteWaypointTrajectoryHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/execute_waypoint_trajectory is not implemented, so the service calls will return the default response."); + } + return true; +} + bool BaseSimulationServices::MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) { @@ -2502,3 +2528,48 @@ bool BaseSimulationServices::GetAllControllerConfigurations(kortex_driver::GetAl } return true; } + +bool BaseSimulationServices::ComputeForwardKinematics(kortex_driver::ComputeForwardKinematics::Request &req, kortex_driver::ComputeForwardKinematics::Response &res) +{ + + + if (ComputeForwardKinematicsHandler) + { + res = ComputeForwardKinematicsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/compute_forward_kinematics is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ComputeInverseKinematics(kortex_driver::ComputeInverseKinematics::Request &req, kortex_driver::ComputeInverseKinematics::Response &res) +{ + + + if (ComputeInverseKinematicsHandler) + { + res = ComputeInverseKinematicsHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/compute_inverse_kinematics is not implemented, so the service calls will return the default response."); + } + return true; +} + +bool BaseSimulationServices::ValidateWaypointList(kortex_driver::ValidateWaypointList::Request &req, kortex_driver::ValidateWaypointList::Response &res) +{ + + + if (ValidateWaypointListHandler) + { + res = ValidateWaypointListHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for base/validate_waypoint_list is not implemented, so the service calls will return the default response."); + } + return true; +} diff --git a/kortex_driver/src/generated/simulation/controlconfig_services.cpp b/kortex_driver/src/generated/simulation/controlconfig_services.cpp index 1dfd2669..6d595c71 100644 --- a/kortex_driver/src/generated/simulation/controlconfig_services.cpp +++ b/kortex_driver/src/generated/simulation/controlconfig_services.cpp @@ -48,6 +48,8 @@ ControlConfigSimulationServices::ControlConfigSimulationServices(ros::NodeHandle m_pub_Error = m_node_handle.advertise("kortex_error", 1000); m_pub_ControlConfigurationTopic = m_node_handle.advertise("control_configuration_topic", 1000); m_is_activated_ControlConfigurationTopic = false; + m_pub_ControlModeTopic = m_node_handle.advertise("control_mode_topic", 1000); + m_is_activated_ControlModeTopic = false; m_serviceSetDeviceID = m_node_handle.advertiseService("control_config/set_device_id", &ControlConfigSimulationServices::SetDeviceID, this); m_serviceSetApiOptions = m_node_handle.advertiseService("control_config/set_api_options", &ControlConfigSimulationServices::SetApiOptions, this); @@ -81,6 +83,7 @@ ControlConfigSimulationServices::ControlConfigSimulationServices(ros::NodeHandle m_serviceResetTwistLinearSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_linear_soft_limit", &ControlConfigSimulationServices::ResetTwistLinearSoftLimit, this); m_serviceResetTwistAngularSoftLimit = m_node_handle.advertiseService("control_config/reset_twist_angular_soft_limit", &ControlConfigSimulationServices::ResetTwistAngularSoftLimit, this); m_serviceResetJointAccelerationSoftLimits = m_node_handle.advertiseService("control_config/reset_joint_acceleration_soft_limits", &ControlConfigSimulationServices::ResetJointAccelerationSoftLimits, this); + m_serviceControlConfig_OnNotificationControlModeTopic = m_node_handle.advertiseService("control_config/activate_publishing_of_control_mode_topic", &ControlConfigSimulationServices::ControlConfig_OnNotificationControlModeTopic, this); } bool ControlConfigSimulationServices::SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) @@ -537,3 +540,25 @@ bool ControlConfigSimulationServices::ResetJointAccelerationSoftLimits(kortex_dr } return true; } + +bool ControlConfigSimulationServices::ControlConfig_OnNotificationControlModeTopic(kortex_driver::ControlConfig_OnNotificationControlModeTopic::Request &req, kortex_driver::ControlConfig_OnNotificationControlModeTopic::Response &res) +{ + + m_is_activated_ControlModeTopic = true; + + if (ControlConfig_OnNotificationControlModeTopicHandler) + { + res = ControlConfig_OnNotificationControlModeTopicHandler(req); + } + else + { + ROS_WARN_ONCE("The simulation handler for control_config/activate_publishing_of_control_mode_topic is not implemented, so the service calls will return the default response."); + } + return true; +} +void ControlConfigSimulationServices::cb_ControlModeTopic(Kinova::Api::ControlConfig::ControlModeNotification notif) +{ + kortex_driver::ControlConfig_ControlModeNotification ros_msg; + ToRosData(notif, ros_msg); + m_pub_ControlModeTopic.publish(ros_msg); +} diff --git a/kortex_driver/src/non-generated/driver/cartesian_trajectory_action_server.cpp b/kortex_driver/src/non-generated/driver/cartesian_trajectory_action_server.cpp new file mode 100644 index 00000000..2699342b --- /dev/null +++ b/kortex_driver/src/non-generated/driver/cartesian_trajectory_action_server.cpp @@ -0,0 +1,453 @@ +/* + * KINOVA (R) KORTEX (TM) + * + * Copyright (c) 2021 Kinova inc. All rights reserved. + * + * This software may be modified and distributed + * under the terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ + +#include "kortex_driver/non-generated/cartesian_trajectory_action_server.h" +#include +#include + +CartesianTrajectoryActionServer::CartesianTrajectoryActionServer(const std::string& server_name, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic): + m_server_name(server_name), + m_node_handle(nh), + m_server(nh, server_name, boost::bind(&CartesianTrajectoryActionServer::goal_received_callback, this, _1), boost::bind(&CartesianTrajectoryActionServer::preempt_received_callback, this, _1), false), + m_base(base), + m_server_state(ActionServerState::INITIALIZING), + m_currentActionID(0) +{ + // Get the ROS params + if (!ros::param::get("~prefix", m_prefix)) + { + std::string error_string = "Prefix name was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } + + // Subscribe to the arm's Action Notifications + m_sub_action_notif_handle = m_base->OnNotificationActionTopic(std::bind(&CartesianTrajectoryActionServer::action_notif_callback, this, std::placeholders::_1), Kinova::Api::Common::NotificationOptions()); + + // Ready to receive goal + m_server.start(); + set_server_state(ActionServerState::IDLE); +} + +CartesianTrajectoryActionServer::~CartesianTrajectoryActionServer() +{ + try + { + m_base->Unsubscribe(m_sub_action_notif_handle); + } + catch (Kinova::Api::KDetailedException& ex) + { + ROS_ERROR("Kortex exception while unsubscribing to action notification."); + ROS_ERROR("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); + ROS_ERROR("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); + ROS_ERROR("Error description: %s\n", ex.what()); + m_goal.setAborted(); + } + catch (std::runtime_error& ex_runtime) + { + ROS_ERROR("Runtime exception detected while unsubscribing to action notification."); + ROS_ERROR("%s", ex_runtime.what()); + m_goal.setAborted(); + } + catch (std::future_error& ex_future) + { + ROS_ERROR("Future exception detected while unsubscribing to action notification."); + ROS_ERROR("%s", ex_future.what()); + m_goal.setAborted(); + } +} + +void CartesianTrajectoryActionServer::goal_received_callback(actionlib::ActionServer::GoalHandle new_goal_handle) +{ + ROS_INFO("New Cartesian goal received."); + if (!is_goal_acceptable(new_goal_handle)) + { + ROS_ERROR("Cartesian Trajectory Goal is rejected."); + new_goal_handle.setRejected(); + return; + } + + if (m_server_state != ActionServerState::IDLE) + { + ROS_WARN("There is already an active cartesian goal. It is being cancelled."); + // We have to call Stop after having received the ACTION_START notification from the arm + stop_all_movement(); + } + + // Accept the goal + ROS_INFO("Cartesian Trajectory Goal is accepted."); + m_goal = new_goal_handle; + m_goal.setAccepted(); + + auto action = Kinova::Api::Base::Action(); + action.set_name("Cartesian waypoint"); + action.set_application_data(""); + + auto proto_trajectory = action.mutable_execute_waypoint_list(); + + proto_trajectory->set_duration(new_goal_handle.getGoal()->goal_time_tolerance.toSec()); + proto_trajectory->set_use_optimal_blending(new_goal_handle.getGoal()->use_optimal_blending); + + for (unsigned int i = 0; i < new_goal_handle.getGoal()->trajectory.size(); i++) + { + const auto traj_point = new_goal_handle.getGoal()->trajectory.at(i); + + Kinova::Api::Base::Waypoint* proto_waypoint = proto_trajectory->add_waypoints(); + proto_waypoint->set_name("waypoint_" + std::to_string(i)); + + auto cartesian_waypoint = proto_waypoint->mutable_cartesian_waypoint(); + + auto tempPose = cartesian_waypoint->mutable_pose(); + tempPose->set_x(traj_point.pose.x); + tempPose->set_y(traj_point.pose.y); + tempPose->set_z(traj_point.pose.z); + + tempPose->set_theta_x(KortexMathUtil::toDeg(traj_point.pose.theta_x)); + tempPose->set_theta_y(KortexMathUtil::toDeg(traj_point.pose.theta_y)); + tempPose->set_theta_z(KortexMathUtil::toDeg(traj_point.pose.theta_z)); + + cartesian_waypoint->set_reference_frame((Kinova::Api::Common::CartesianReferenceFrame)traj_point.reference_frame); + cartesian_waypoint->set_maximum_angular_velocity(traj_point.maximum_angular_velocity); + cartesian_waypoint->set_maximum_linear_velocity(traj_point.maximum_linear_velocity); + cartesian_waypoint->set_blending_radius(traj_point.blending_radius); + } + + try + { + // Validate the waypoints and reject the goal if they fail validation + auto report = m_base->ValidateWaypointList(*proto_trajectory); + if (report.trajectory_error_report().trajectory_error_elements_size() > 0) + { + ROS_ERROR("Cartesian Trajectory failed validation in the arm."); + + // Go through report and print errors + for (unsigned int i = 0; i < report.trajectory_error_report().trajectory_error_elements_size(); i++) + { + ROS_ERROR("Error %i : %s", i+1, report.trajectory_error_report().trajectory_error_elements(i).message().c_str()); + } + new_goal_handle.setRejected(); + m_goal.setAborted(); + return; + } + + // Make sure to clear the faults before moving the robot + m_base->ClearFaults(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // Send the trajectory to the robot + m_base->ExecuteAction(action); + set_server_state(ActionServerState::PRE_PROCESSING_PENDING); + } + catch (Kinova::Api::KDetailedException& ex) + { + ROS_ERROR("Kortex exception while sending the trajectory"); + ROS_ERROR("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); + ROS_ERROR("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); + ROS_ERROR("Error description: %s\n", ex.what()); + m_goal.setAborted(); + } + catch (std::runtime_error& ex_runtime) + { + ROS_ERROR("Runtime exception detected while sending the trajectory"); + ROS_ERROR("%s", ex_runtime.what()); + m_goal.setAborted(); + } + catch (std::future_error& ex_future) + { + ROS_ERROR("Future exception detected while sending the trajectory"); + ROS_ERROR("%s", ex_future.what()); + m_goal.setAborted(); + } +} + +// Called in a separate thread when a preempt request comes in from the Action Client +void CartesianTrajectoryActionServer::preempt_received_callback(actionlib::ActionServer::GoalHandle goal_handle) +{ + if (m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS) + { + stop_all_movement(); + } +} + +// Called in a separate thread when a notification comes in +void CartesianTrajectoryActionServer::action_notif_callback(Kinova::Api::Base::ActionNotification notif) +{ + if(m_server_state == ActionServerState::IDLE) + { + return; + } + Kinova::Api::Base::ActionEvent event = notif.action_event(); + Kinova::Api::Base::ActionHandle handle = notif.handle(); + Kinova::Api::Base::ActionType type = handle.action_type(); + ROS_DEBUG("Action notification received of type %s", Kinova::Api::Base::ActionEvent_Name(event).c_str()); + + std::lock_guard guard(m_action_notification_thread_lock); + + kortex_driver::FollowCartesianTrajectoryResult result; + std::ostringstream oss; + + if (type == Kinova::Api::Base::ActionType::EXECUTE_WAYPOINT_LIST) + { + if(event == Kinova::Api::Base::ActionEvent::ACTION_PREPROCESS_START) + { + m_currentActionID = notif.handle().identifier(); + + // It should be starting + if (m_server_state == ActionServerState::PRE_PROCESSING_PENDING) + { + ROS_INFO("Preprocessing has started in the arm."); + set_server_state(ActionServerState::PRE_PROCESSING_IN_PROGRESS); + } + // We should not have received that + else + { + ROS_DEBUG("Notification mismatch : received ACTION_PREPROCESS_START but we are in %s", actionServerStateNames[int(m_server_state)]); + } + } + else if(m_currentActionID == notif.handle().identifier()) + { + switch (event) + { + + // The pre-processing has ended successfully in the arm + case Kinova::Api::Base::ActionEvent::ACTION_PREPROCESS_END: + // It was ongoing and now it ended + if (m_server_state == ActionServerState::PRE_PROCESSING_PENDING || + m_server_state == ActionServerState::PRE_PROCESSING_IN_PROGRESS) + { + ROS_INFO("Preprocessing has finished in the arm and goal has been accepted."); + set_server_state(ActionServerState::TRAJECTORY_EXECUTION_PENDING); + } + // FIXME KOR-3563 Sometimes the notifications arrive in the wrong order so it is possible to receive + // a ACTION_PREPROCESS_END notification after the ACTION_START + // When this bug will be fixed this else if can be removed + else if (m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS) + { + ROS_DEBUG("Notification order mismatch : We received the ACTION_PREPROCESS_END after the ACTION_START"); + break; + } + // We should not have received that + else + { + ROS_DEBUG("Notification mismatch : received ACTION_PREPROCESS_END but we are in %s", actionServerStateNames[int(m_server_state)]); + } + break; + + // The pre-processing has failed in the arm + // TODO This could be much lighter since ValidateWaypoints already does that + case Kinova::Api::Base::ActionEvent::ACTION_PREPROCESS_ABORT: + // It was ongoing and now it ended (and failed) + if ((m_server_state == ActionServerState::PRE_PROCESSING_IN_PROGRESS)) + { + ROS_DEBUG("Preprocessing has finished in the arm and goal has been rejected. Fetching the error report from the arm..."); + + result.error_code = result.INVALID_GOAL; + + // Get the error report and show errors here + Kinova::Api::Base::TrajectoryErrorReport report = m_base->GetTrajectoryErrorReport(); + oss << "Error report has been fetched and error elements are listed below : " << std::endl; + int i = 1; + for (auto error_element : report.trajectory_error_elements()) + { + oss << "-----------------------------" << std::endl; + oss << "Error #" << i << std::endl; + oss << "Type : " << Kinova::Api::Base::TrajectoryErrorType_Name(error_element.error_type()) << std::endl; + oss << "Erroneous value is " << error_element.error_value() << " but minimum permitted is " << error_element.min_value() << " and maximum permitted is " << error_element.max_value() << std::endl; + if (error_element.message() != "") + { + oss << "Additional message is : " << error_element.message() << std::endl; + } + oss << "-----------------------------" << std::endl; + + i++; + } + + ROS_ERROR("%s", oss.str().c_str()); + + result.error_string = oss.str(); + m_goal.setAborted(result); + + set_server_state(ActionServerState::IDLE); + } + // We should not have received that + else + { + ROS_DEBUG("Notification mismatch : received ACTION_PREPROCESS_ABORT but we are in %s", actionServerStateNames[int(m_server_state)]); + } + break; + + // The arm is starting to move + case Kinova::Api::Base::ActionEvent::ACTION_START: + // The preprocessing was done and the goal is still active (not preempted) + if ((m_server_state == ActionServerState::TRAJECTORY_EXECUTION_PENDING || + m_server_state == ActionServerState::PRE_PROCESSING_IN_PROGRESS) && // FIXME KOR-3563 this happens if we received a ACTION_START before a ACTION_PREPROCESS_END + m_goal.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE) + { + ROS_INFO("Trajectory has started."); + set_server_state(ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS); + // Remember when the trajectory started + m_trajectory_start_time = std::chrono::system_clock::now(); + } + // The preprocessing was done but the goal put to "PREEMPTING" by the client while preprocessing + // The stop_all_movement() call will trigger a ACTION_ABORT notification + else if ((m_server_state == ActionServerState::TRAJECTORY_EXECUTION_PENDING) && + m_goal.getGoalStatus().status == actionlib_msgs::GoalStatus::PREEMPTING) + { + ROS_INFO("Trajectory has started but goal was cancelled : stopping all movement."); + stop_all_movement(); + } + // We should not have received that + else + { + ROS_DEBUG("Notification mismatch : received ACTION_START but we are in %s", actionServerStateNames[int(m_server_state)]); + } + break; + + case Kinova::Api::Base::ActionEvent::ACTION_FEEDBACK: + { + // debug trace to indicate we've reached waypoints + for (unsigned int i = 0; i < notif.trajectory_info_size(); i++) + { + auto info = notif.trajectory_info(i); + if (info.trajectory_info_type() == Kinova::Api::Base::TrajectoryInfoType::WAYPOINT_REACHED) + { + ROS_DEBUG("Cartesian waypoint %d reached", info.waypoint_index()); + } + } + break; + } + + // The action was started in the arm, but it aborted + case Kinova::Api::Base::ActionEvent::ACTION_ABORT: + // The goal is still active, but we received a ABORT before starting, or during execution + if (m_goal.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE && + (m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS || + m_server_state == ActionServerState::TRAJECTORY_EXECUTION_PENDING)) + { + ROS_ERROR("Trajectory has been aborted."); + + result.error_code = result.PATH_TOLERANCE_VIOLATED; + oss << "Trajectory execution failed in the arm with sub error code " << notif.abort_details() << std::endl; + if (notif.abort_details() == Kinova::Api::SubErrorCodes::CONTROL_WRONG_STARTING_POINT) + { + oss << "The starting point for the trajectory did not match the actual commanded cartesian pose." << std::endl; + } + else if (notif.abort_details() == Kinova::Api::SubErrorCodes::CONTROL_MANUAL_STOP) + { + oss << "The speed while executing the trajectory was too damn high and caused the robot to stop." << std::endl; + } + result.error_string = oss.str(); + m_goal.setAborted(result); + + ROS_ERROR("%s", oss.str().c_str()); + set_server_state(ActionServerState::IDLE); + } + // The goal was cancelled and we received a ACTION_ABORT : this means the trajectory was cancelled successfully in the arm + else if (m_goal.getGoalStatus().status == actionlib_msgs::GoalStatus::PREEMPTING && + (m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS || + m_server_state == ActionServerState::TRAJECTORY_EXECUTION_PENDING)) + { + ROS_INFO("Trajectory has been cancelled successfully in the arm."); + m_goal.setCanceled(); + set_server_state(ActionServerState::IDLE); + } + // We should not have received that + else + { + ROS_DEBUG("Notification mismatch : received ACTION_ABORT but we are in %s", actionServerStateNames[int(m_server_state)]); + } + break; + + // The trajectory just ended + case Kinova::Api::Base::ActionEvent::ACTION_END: + { + // The trajectory was ongoing + if ((m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS)) + { + result.error_code = result.SUCCESSFUL; + ROS_INFO("Trajectory execution succeeded."); + m_goal.setSucceeded(result); + + set_server_state(ActionServerState::IDLE); + } + // We should not have received that + else + { + ROS_DEBUG("Notification mismatch : received ACTION_END but we are in %s", actionServerStateNames[int(m_server_state)]); + } + break; + } + + case Kinova::Api::Base::ActionEvent::ACTION_PAUSE: + ROS_WARN("Action pause event was just received and this should never happen."); + break; + + default: + ROS_WARN("Unknown action event was just received and this should never happen."); + break; + } + } + + + } + // Wrong action type. Rejecting the notification. Action server state unchanged. + else + { + return; + } + + oss.flush(); +} + +bool CartesianTrajectoryActionServer::is_goal_acceptable(actionlib::ActionServer::GoalHandle goal_handle) +{ + // First check if goal is valid + if (!goal_handle.isValid()) + { + ROS_ERROR("Invalid Cartesian goal."); + return false; + } + + // Retrieve the goal + kortex_driver::FollowCartesianTrajectoryGoalConstPtr goal = goal_handle.getGoal(); + + // Check if the trajectory contains at least 1 waypoint. + if (goal->trajectory.size() == 0) + { + ROS_ERROR("Empty Cartesian trajectory list."); + return false; + } + + return true; +} + +void CartesianTrajectoryActionServer::stop_all_movement() +{ + ROS_INFO("Calling Stop on the robot."); + try + { + m_base->Stop(); + } + catch(const Kinova::Api::KBasicException& e) + { + ROS_WARN("Stop failed : %s", e.what()); + } +} + +void CartesianTrajectoryActionServer::set_server_state(ActionServerState s) +{ + std::lock_guard guard(m_server_state_lock); + ActionServerState old_state = m_server_state; + m_server_state = s; + ROS_INFO("State changed from %s to %s\n", actionServerStateNames[int(old_state)], actionServerStateNames[int(s)]); +} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/driver/pre_computed_joint_trajectory_action_server.cpp b/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp similarity index 60% rename from kortex_driver/src/non-generated/driver/pre_computed_joint_trajectory_action_server.cpp rename to kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp index 10923219..94bf1c20 100644 --- a/kortex_driver/src/non-generated/driver/pre_computed_joint_trajectory_action_server.cpp +++ b/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp @@ -10,17 +10,23 @@ * */ -#include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" +#include "kortex_driver/non-generated/joint_trajectory_action_server.h" #include #include -PreComputedJointTrajectoryActionServer::PreComputedJointTrajectoryActionServer(const std::string& server_name, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic): +namespace { + constexpr float STARTING_POINT_ARBITRARY_DURATION = 0.5f; +} + +JointTrajectoryActionServer::JointTrajectoryActionServer(const std::string& server_name, ros::NodeHandle& nh, Kinova::Api::Base::BaseClient* base, Kinova::Api::BaseCyclic::BaseCyclicClient* base_cyclic, Kinova::Api::ControlConfig::ControlConfigClient* control_config, bool use_hard_limits): m_server_name(server_name), m_node_handle(nh), - m_server(nh, server_name, boost::bind(&PreComputedJointTrajectoryActionServer::goal_received_callback, this, _1), boost::bind(&PreComputedJointTrajectoryActionServer::preempt_received_callback, this, _1), false), + m_server(nh, server_name, boost::bind(&JointTrajectoryActionServer::goal_received_callback, this, _1), boost::bind(&JointTrajectoryActionServer::preempt_received_callback, this, _1), false), m_base(base), m_base_cyclic(base_cyclic), - m_server_state(ActionServerState::INITIALIZING) + m_control_config(control_config), + m_server_state(ActionServerState::INITIALIZING), + m_use_hard_limits(use_hard_limits) { // Get the ROS params if (!ros::param::get("~default_goal_time_tolerance", m_default_goal_time_tolerance)) @@ -45,21 +51,24 @@ PreComputedJointTrajectoryActionServer::PreComputedJointTrajectoryActionServer(c ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + + // Get the hard limits + m_hard_limits = m_control_config->GetKinematicHardLimits(); // Subscribe to the arm's Action Notifications - m_sub_action_notif_handle = m_base->OnNotificationActionTopic(std::bind(&PreComputedJointTrajectoryActionServer::action_notif_callback, this, std::placeholders::_1), Kinova::Api::Common::NotificationOptions()); + m_sub_action_notif_handle = m_base->OnNotificationActionTopic(std::bind(&JointTrajectoryActionServer::action_notif_callback, this, std::placeholders::_1), Kinova::Api::Common::NotificationOptions()); // Ready to receive goal m_server.start(); set_server_state(ActionServerState::IDLE); } -PreComputedJointTrajectoryActionServer::~PreComputedJointTrajectoryActionServer() +JointTrajectoryActionServer::~JointTrajectoryActionServer() { m_base->Unsubscribe(m_sub_action_notif_handle); } -void PreComputedJointTrajectoryActionServer::goal_received_callback(actionlib::ActionServer::GoalHandle new_goal_handle) +void JointTrajectoryActionServer::goal_received_callback(actionlib::ActionServer::GoalHandle new_goal_handle) { ROS_INFO("New goal received."); if (!is_goal_acceptable(new_goal_handle)) @@ -76,65 +85,123 @@ void PreComputedJointTrajectoryActionServer::goal_received_callback(actionlib::A stop_all_movement(); } - // Make sure to clear the faults before moving the robot - m_base->ClearFaults(); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - // Accept the goal ROS_INFO("Joint Trajectory Goal is accepted."); m_goal = new_goal_handle; m_goal.setAccepted(); - // Check if we are already there physically - if (is_goal_tolerance_respected(false, false)) - { - ROS_INFO("We already reached the goal position : nothing to do."); - m_goal.setSucceeded(); - return; - } - // Construct the Protobuf object trajectory - trajectory_msgs::JointTrajectory ros_trajectory = m_goal.getGoal()->trajectory; - - Kinova::Api::Base::PreComputedJointTrajectory proto_trajectory; - - // For logging purposes - std::ofstream myfile; - myfile.open ("moveit_trajectory.csv"); - - // Set the continuity mode - proto_trajectory.set_mode(Kinova::Api::Base::TrajectoryContinuityMode::TRAJECTORY_CONTINUITY_MODE_POSITION); + Kinova::Api::Base::WaypointList proto_trajectory; + + // Uncomment for logging purposes + // std::ofstream myfile; + // myfile.open ("/tmp/moveit_trajectory.csv"); + // for (unsigned int i = 0; i < new_goal_handle.getGoal()->trajectory.points.size(); i++) + // { + // myfile << new_goal_handle.getGoal()->trajectory.points[i].time_from_start.toSec() << ","; + // for (auto position : new_goal_handle.getGoal()->trajectory.points[i].positions) + // { + // myfile << position << ","; + // } + // myfile << "\n"; + // } + // myfile.close(); // Copy the trajectory points from the ROS structure to the Protobuf structure - for (auto traj_point : m_goal.getGoal()->trajectory.points) + for (unsigned int i = 0; i < new_goal_handle.getGoal()->trajectory.points.size(); i++) { - Kinova::Api::Base::PreComputedJointTrajectoryElement* proto_element = proto_trajectory.add_trajectory_elements(); - myfile << traj_point.time_from_start.toSec() << ","; - for (auto position : traj_point.positions) + // Create the waypoint + Kinova::Api::Base::Waypoint* proto_waypoint = proto_trajectory.add_waypoints(); + proto_waypoint->set_name("waypoint_" + std::to_string(i)); + auto angular_waypoint = proto_waypoint->mutable_angular_waypoint(); + + // Calculate the duration of this waypoint + const auto traj_point = new_goal_handle.getGoal()->trajectory.points.at(i); + float waypoint_duration; + // FIXME We have to include the starting point for the trajectory as the first waypoint AND since optimal durations are not + // supported yet, we also have to give it a non-zero duration. Putting something too small gives errors so I've put + // an arbitrary value of 500ms here for now. + if (i == 0) { - proto_element->add_joint_angles(m_math_util.toDeg(position)); - myfile << position << ","; + waypoint_duration = STARTING_POINT_ARBITRARY_DURATION; } - for (auto velocity : traj_point.velocities) + else { - proto_element->add_joint_speeds(m_math_util.toDeg(velocity)); - myfile << velocity << ","; + auto previous_wp_duration = new_goal_handle.getGoal()->trajectory.points.at(i-1).time_from_start.toSec(); + waypoint_duration = traj_point.time_from_start.toSec() - previous_wp_duration; + // Very small durations are rejected so we cap them + if (waypoint_duration < 0.01) + { + waypoint_duration = 0.01f; + } } - for (auto acceleration : traj_point.accelerations) + + // Fill the waypoint + for (auto position : traj_point.positions) { - proto_element->add_joint_accelerations(m_math_util.toDeg(acceleration)); - myfile << acceleration << ","; + angular_waypoint->add_angles(KortexMathUtil::toDeg(position)); } - myfile << "\n"; - proto_element->set_time_from_start(traj_point.time_from_start.toSec()); + angular_waypoint->set_duration(waypoint_duration); } - myfile.close(); + // If the hard limits are used for the trajectory + if (m_use_hard_limits) + { + // Get the soft limits + m_soft_limits = getAngularTrajectorySoftLimits(); - // Send the trajectory to the robot + // Set the soft limits to hard limits + setAngularTrajectorySoftLimitsToMax(); + } + + try + { + // Validate the waypoints and reject the goal if they fail validation + auto report = m_base->ValidateWaypointList(proto_trajectory); + + if (report.trajectory_error_report().trajectory_error_elements_size() > 0) + { + ROS_ERROR("Joint Trajectory failed validation in the arm."); + // Go through report and print errors + for (unsigned int i = 0; i < report.trajectory_error_report().trajectory_error_elements_size(); i++) + { + ROS_ERROR("Error %i : %s", i+1, report.trajectory_error_report().trajectory_error_elements(i).message().c_str()); + } + setAngularTrajectorySoftLimits(m_soft_limits); + new_goal_handle.setAborted(); + return; + } + } + catch (Kinova::Api::KDetailedException& ex) + { + ROS_ERROR("Kortex exception while validating the trajectory"); + ROS_ERROR("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); + ROS_ERROR("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); + ROS_ERROR("Error description: %s\n", ex.what()); + m_goal.setAborted(); + } + catch (std::runtime_error& ex_runtime) + { + ROS_ERROR("Runtime exception detected while sending the trajectory"); + ROS_ERROR("%s", ex_runtime.what()); + m_goal.setAborted(); + } + catch (std::future_error& ex_future) + { + ROS_ERROR("Future exception detected while getting feedback"); + ROS_ERROR("%s", ex_future.what()); + m_goal.setAborted(); + } + try { - m_base->PlayPreComputedJointTrajectory(proto_trajectory); + // Make sure to clear the faults before moving the robot + m_base->ClearFaults(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // Send the trajectory to the robot + m_base->ExecuteWaypointTrajectory(proto_trajectory); set_server_state(ActionServerState::PRE_PROCESSING_PENDING); } catch (Kinova::Api::KDetailedException& ex) @@ -143,24 +210,27 @@ void PreComputedJointTrajectoryActionServer::goal_received_callback(actionlib::A ROS_ERROR("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); ROS_ERROR("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); ROS_ERROR("Error description: %s\n", ex.what()); + setAngularTrajectorySoftLimits(m_soft_limits); m_goal.setAborted(); } catch (std::runtime_error& ex_runtime) { ROS_ERROR("Runtime exception detected while sending the trajectory"); ROS_ERROR("%s", ex_runtime.what()); + setAngularTrajectorySoftLimits(m_soft_limits); m_goal.setAborted(); } catch (std::future_error& ex_future) { ROS_ERROR("Future exception detected while getting feedback"); ROS_ERROR("%s", ex_future.what()); + setAngularTrajectorySoftLimits(m_soft_limits); m_goal.setAborted(); } } // Called in a separate thread when a preempt request comes in from the Action Client -void PreComputedJointTrajectoryActionServer::preempt_received_callback(actionlib::ActionServer::GoalHandle goal_handle) +void JointTrajectoryActionServer::preempt_received_callback(actionlib::ActionServer::GoalHandle goal_handle) { if (m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS) { @@ -169,7 +239,7 @@ void PreComputedJointTrajectoryActionServer::preempt_received_callback(actionlib } // Called in a separate thread when a notification comes in -void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api::Base::ActionNotification notif) +void JointTrajectoryActionServer::action_notif_callback(Kinova::Api::Base::ActionNotification notif) { Kinova::Api::Base::ActionEvent event = notif.action_event(); Kinova::Api::Base::ActionHandle handle = notif.handle(); @@ -180,7 +250,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: control_msgs::FollowJointTrajectoryResult result; std::ostringstream oss; - if (type == Kinova::Api::Base::ActionType::PLAY_PRE_COMPUTED_TRAJECTORY) + if (type == Kinova::Api::Base::ActionType::EXECUTE_WAYPOINT_LIST) { switch (event) { @@ -195,7 +265,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: // We should not have received that else { - ROS_ERROR("Notification mismatch : received ACTION_PREPROCESS_START but we are in %s", actionServerStateNames[int(m_server_state)]); + ROS_DEBUG("Notification mismatch : received ACTION_PREPROCESS_START but we are in %s", actionServerStateNames[int(m_server_state)]); } break; @@ -219,7 +289,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: // We should not have received that else { - ROS_ERROR("Notification mismatch : received ACTION_PREPROCESS_END but we are in %s", actionServerStateNames[int(m_server_state)]); + ROS_DEBUG("Notification mismatch : received ACTION_PREPROCESS_END but we are in %s", actionServerStateNames[int(m_server_state)]); } break; @@ -241,7 +311,6 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: oss << "-----------------------------" << std::endl; oss << "Error #" << i << std::endl; oss << "Type : " << Kinova::Api::Base::TrajectoryErrorType_Name(error_element.error_type()) << std::endl; - oss << "Identifier : " << Kinova::Api::Base::TrajectoryErrorIdentifier_Name(error_element.error_identifier()) << std::endl; oss << "Actuator : " << error_element.index()+1 << std::endl; oss << "Erroneous value is " << error_element.error_value() << " but minimum permitted is " << error_element.min_value() << " and maximum permitted is " << error_element.max_value() << std::endl; if (error_element.message() != "") @@ -263,7 +332,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: // We should not have received that else { - ROS_ERROR("Notification mismatch : received ACTION_PREPROCESS_ABORT but we are in %s", actionServerStateNames[int(m_server_state)]); + ROS_DEBUG("Notification mismatch : received ACTION_PREPROCESS_ABORT but we are in %s", actionServerStateNames[int(m_server_state)]); } break; @@ -290,10 +359,24 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: // We should not have received that else { - ROS_ERROR("Notification mismatch : received ACTION_START but we are in %s", actionServerStateNames[int(m_server_state)]); + ROS_DEBUG("Notification mismatch : received ACTION_START but we are in %s", actionServerStateNames[int(m_server_state)]); } break; + case Kinova::Api::Base::ActionEvent::ACTION_FEEDBACK: + { + // debug trace to indicate we've reached waypoints + for (unsigned int i = 0; i < notif.trajectory_info_size(); i++) + { + auto info = notif.trajectory_info(i); + if (info.trajectory_info_type() == Kinova::Api::Base::TrajectoryInfoType::WAYPOINT_REACHED) + { + ROS_DEBUG("Waypoint %d reached", info.waypoint_index()); + } + } + break; + } + // The action was started in the arm, but it aborted case Kinova::Api::Base::ActionEvent::ACTION_ABORT: // The goal is still active, but we received a ABORT before starting, or during execution @@ -331,7 +414,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: // We should not have received that else { - ROS_ERROR("Notification mismatch : received ACTION_ABORT but we are in %s", actionServerStateNames[int(m_server_state)]); + ROS_DEBUG("Notification mismatch : received ACTION_ABORT but we are in %s", actionServerStateNames[int(m_server_state)]); } break; @@ -343,6 +426,11 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: { ROS_INFO("Trajectory has finished in the arm."); m_trajectory_end_time = std::chrono::system_clock::now(); + // When going at full speed we have to stabilize a bit before checking the final position + if (m_use_hard_limits) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } bool is_tolerance_respected = is_goal_tolerance_respected(true, true); if (is_tolerance_respected) { @@ -364,17 +452,17 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: // We should not have received that else { - ROS_ERROR("Notification mismatch : received ACTION_END but we are in %s", actionServerStateNames[int(m_server_state)]); + ROS_DEBUG("Notification mismatch : received ACTION_END but we are in %s", actionServerStateNames[int(m_server_state)]); } break; } case Kinova::Api::Base::ActionEvent::ACTION_PAUSE: - ROS_WARN("Action pause event was just received and this should never happen."); + ROS_DEBUG("Action pause event was just received and this should never happen."); break; default: - ROS_WARN("Unknown action event was just received and this should never happen."); + ROS_DEBUG("Unknown action event was just received and this should never happen."); break; } } @@ -387,7 +475,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api:: oss.flush(); } -bool PreComputedJointTrajectoryActionServer::is_goal_acceptable(actionlib::ActionServer::GoalHandle goal_handle) +bool JointTrajectoryActionServer::is_goal_acceptable(actionlib::ActionServer::GoalHandle goal_handle) { // First check if goal is valid if (!goal_handle.isValid()) @@ -424,38 +512,34 @@ bool PreComputedJointTrajectoryActionServer::is_goal_acceptable(actionlib::Actio std::cout << std::endl; return false; } + + return true; +} - // Goal needs to have 1msec timesteps intervals between all trajectory points to not be rejected - double difference = 0.0; - bool result = true; - trajectory_msgs::JointTrajectoryPoint traj_point; - for (int i = 1; i < goal->trajectory.points.size() && result; i++) +bool JointTrajectoryActionServer::is_goal_tolerance_respected(bool enable_prints, bool check_time_tolerance) +{ + // Get feedback from arm (retry if timeouts occur) + bool is_goal_respected = true; + Kinova::Api::BaseCyclic::Feedback feedback; + bool got_feedback = true; + while (!got_feedback) { - difference = goal->trajectory.points.at(i).time_from_start.toSec() - goal->trajectory.points.at(i-1).time_from_start.toSec(); - if (i > 0 && i < goal->trajectory.points.size()-1) + try + { + feedback = m_base_cyclic->RefreshFeedback(); + got_feedback = true; + } + catch(const std::exception& e) { - result = (fabs(difference-0.001) < 10.0*FLT_EPSILON); } } - if(!result) - { - ROS_ERROR("Insufficient point spacing."); - } - return result; -} - -bool PreComputedJointTrajectoryActionServer::is_goal_tolerance_respected(bool enable_prints, bool check_time_tolerance) -{ - // Get feedback from arm - bool is_goal_respected = true; - Kinova::Api::BaseCyclic::Feedback feedback = m_base_cyclic->RefreshFeedback(); auto goal = m_goal.getGoal(); // Check the goal_time_tolerance for trajectory execution if (check_time_tolerance) { double actual_trajectory_duration = std::chrono::duration(m_trajectory_end_time - m_trajectory_start_time).count(); - double desired_trajectory_duration = goal->trajectory.points.at(goal->trajectory.points.size()-1).time_from_start.toSec(); + double desired_trajectory_duration = goal->trajectory.points.at(goal->trajectory.points.size()-1).time_from_start.toSec() + STARTING_POINT_ARBITRARY_DURATION; double time_tolerance = goal->goal_time_tolerance.toSec() == 0.0 ? m_default_goal_time_tolerance : goal->goal_time_tolerance.toSec(); if (actual_trajectory_duration > desired_trajectory_duration + time_tolerance ) { @@ -495,7 +579,7 @@ bool PreComputedJointTrajectoryActionServer::is_goal_tolerance_respected(bool en for (auto act: feedback.actuators()) { double actual_position = act.position(); // in degrees - double desired_position = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(goal->trajectory.points.at(goal->trajectory.points.size()-1).positions[current_index])); + double desired_position = KortexMathUtil::wrapDegreesFromZeroTo360(KortexMathUtil::toDeg(goal->trajectory.points.at(goal->trajectory.points.size()-1).positions[current_index])); double tolerance = 0.0; if (goal_tolerances[current_index] == -1.0) @@ -505,10 +589,10 @@ bool PreComputedJointTrajectoryActionServer::is_goal_tolerance_respected(bool en } else { - tolerance = m_math_util.toDeg(goal_tolerances[current_index]); + tolerance = KortexMathUtil::toDeg(goal_tolerances[current_index]); } - double error = m_math_util.wrapDegreesFromZeroTo360(std::min(fabs(actual_position - desired_position), fabs(fabs(actual_position - desired_position) - 360.0))); + double error = KortexMathUtil::wrapDegreesFromZeroTo360(std::min(fabs(actual_position - desired_position), fabs(fabs(actual_position - desired_position) - 360.0))); if (error > tolerance) { is_goal_respected = false; @@ -521,7 +605,7 @@ bool PreComputedJointTrajectoryActionServer::is_goal_tolerance_respected(bool en return is_goal_respected; } -void PreComputedJointTrajectoryActionServer::stop_all_movement() +void JointTrajectoryActionServer::stop_all_movement() { ROS_INFO("Calling Stop on the robot."); try @@ -534,10 +618,99 @@ void PreComputedJointTrajectoryActionServer::stop_all_movement() } } -void PreComputedJointTrajectoryActionServer::set_server_state(ActionServerState s) +void JointTrajectoryActionServer::set_server_state(ActionServerState s) { std::lock_guard guard(m_server_state_lock); ActionServerState old_state = m_server_state; m_server_state = s; + // If we're going to IDLE, set back the soft limits + if (s == ActionServerState::IDLE) + { + setAngularTrajectorySoftLimits(m_soft_limits); + } ROS_INFO("State changed from %s to %s\n", actionServerStateNames[int(old_state)], actionServerStateNames[int(s)]); } + +AngularTrajectorySoftLimits JointTrajectoryActionServer::getAngularTrajectorySoftLimits() +{ + const Kinova::Api::ControlConfig::ControlMode target_control_mode = Kinova::Api::ControlConfig::ANGULAR_TRAJECTORY; + + Kinova::Api::ControlConfig::ControlModeInformation info; + info.set_control_mode(target_control_mode); + auto soft_limits = m_control_config->GetKinematicSoftLimits(info); + + // Set Joint Speed limits to max + Kinova::Api::ControlConfig::JointSpeedSoftLimits vel; + vel.set_control_mode(target_control_mode); + for (auto j : soft_limits.joint_speed_limits()) + { + vel.add_joint_speed_soft_limits(j); + } + + // Set Joint Acceleration limits to max + Kinova::Api::ControlConfig::JointAccelerationSoftLimits acc; + acc.set_control_mode(target_control_mode); + for (auto j : soft_limits.joint_acceleration_limits()) + { + acc.add_joint_acceleration_soft_limits(j); + } + + return AngularTrajectorySoftLimits(vel, acc); +} + +void JointTrajectoryActionServer::setAngularTrajectorySoftLimitsToMax() +{ + ROS_DEBUG("Setting soft limits to hard"); + try + { + const Kinova::Api::ControlConfig::ControlMode target_control_mode = Kinova::Api::ControlConfig::ANGULAR_TRAJECTORY; + + // Set Joint Speed limits to max + Kinova::Api::ControlConfig::JointSpeedSoftLimits jpsl; + jpsl.set_control_mode(target_control_mode); + for (auto j : m_hard_limits.joint_speed_limits()) + { + jpsl.add_joint_speed_soft_limits(j); + } + m_control_config->SetJointSpeedSoftLimits(jpsl); + + // Set Joint Acceleration limits to max + Kinova::Api::ControlConfig::JointAccelerationSoftLimits jasl; + jasl.set_control_mode(target_control_mode); + for (auto j : m_hard_limits.joint_acceleration_limits()) + { + jasl.add_joint_acceleration_soft_limits(j); + } + m_control_config->SetJointAccelerationSoftLimits(jasl); + } + catch (Kinova::Api::KDetailedException& ex) + { + ROS_WARN("Kortex exception while setting the angular soft limits"); + ROS_WARN("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); + ROS_WARN("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); + ROS_WARN("Error description: %s\n", ex.what()); + } +} + +void JointTrajectoryActionServer::setAngularTrajectorySoftLimits(const AngularTrajectorySoftLimits& limits) +{ + ROS_DEBUG("Setting back soft limits"); + if (!limits.empty()) + { + try + { + // Set Joint Speed limits + m_control_config->SetJointSpeedSoftLimits(limits.m_vel); + + // Set Joint Acceleration limits to max + m_control_config->SetJointAccelerationSoftLimits(limits.m_acc); + } + catch (Kinova::Api::KDetailedException& ex) + { + ROS_WARN("Kortex exception while setting the angular soft limits"); + ROS_WARN("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); + ROS_WARN("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); + ROS_WARN("Error description: %s\n", ex.what()); + } + } +} diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index e266547e..cbfc48b3 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -87,6 +87,8 @@ KortexArmDriver::~KortexArmDriver() { delete m_action_server_follow_joint_trajectory; + delete m_action_server_follow_cartesian_trajectory; + if (m_action_server_gripper_command) { delete m_action_server_gripper_command; @@ -337,11 +339,6 @@ void KortexArmDriver::verifyProductConfiguration() ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } - // Set Angular Trajectories soft limits to max if the option is true in the launch file - if (m_use_hard_limits) - { - setAngularTrajectorySoftLimitsToMax(); - } } else if (m_arm_name == "gen3_lite") { @@ -517,8 +514,11 @@ void KortexArmDriver::initRosServices() void KortexArmDriver::startActionServers() { // Start Action servers - m_action_server_follow_joint_trajectory = new PreComputedJointTrajectoryActionServer(m_arm_name + "_joint_trajectory_controller/follow_joint_trajectory", m_node_handle, m_base, m_base_cyclic); + m_action_server_follow_joint_trajectory = new JointTrajectoryActionServer(m_arm_name + "_joint_trajectory_controller/follow_joint_trajectory", m_node_handle, m_base, m_base_cyclic, m_control_config, m_use_hard_limits); // Start Gripper Action Server if the arm has a gripper + + m_action_server_follow_cartesian_trajectory = new CartesianTrajectoryActionServer("cartesian_trajectory_controller/follow_cartesian_trajectory", m_node_handle, m_base, m_base_cyclic); + m_action_server_gripper_command = nullptr; if (isGripperPresent()) { @@ -531,40 +531,6 @@ bool KortexArmDriver::isGripperPresent() return m_gripper_name != ""; } -void KortexArmDriver::setAngularTrajectorySoftLimitsToMax() -{ - try - { - auto hard_limits = m_control_config->GetKinematicHardLimits(); - Kinova::Api::ControlConfig::ControlMode target_control_mode = Kinova::Api::ControlConfig::ANGULAR_TRAJECTORY; - - // Set Joint Speed limits to max - Kinova::Api::ControlConfig::JointSpeedSoftLimits jpsl; - jpsl.set_control_mode(target_control_mode); - for (auto j : hard_limits.joint_speed_limits()) - { - jpsl.add_joint_speed_soft_limits(j); - } - m_control_config->SetJointSpeedSoftLimits(jpsl); - - // Set Joint Acceleration limits to max - Kinova::Api::ControlConfig::JointAccelerationSoftLimits jasl; - jasl.set_control_mode(target_control_mode); - for (auto j : hard_limits.joint_acceleration_limits()) - { - jasl.add_joint_acceleration_soft_limits(j); - } - m_control_config->SetJointAccelerationSoftLimits(jasl); - } - catch (Kinova::Api::KDetailedException& ex) - { - ROS_WARN("Kortex exception while getting the base_feedback"); - ROS_WARN("Error code: %s\n", Kinova::Api::ErrorCodes_Name(ex.getErrorInfo().getError().error_code()).c_str()); - ROS_WARN("Error sub code: %s\n", Kinova::Api::SubErrorCodes_Name(Kinova::Api::SubErrorCodes(ex.getErrorInfo().getError().error_sub_code())).c_str()); - ROS_WARN("Error description: %s\n", ex.what()); - } -} - void KortexArmDriver::publishRobotFeedback() { @@ -616,7 +582,7 @@ void KortexArmDriver::publishRobotFeedback() for (int i = 0; i < base_feedback.actuators.size(); i++) { - joint_state.name[i] = m_arm_joint_names[i]; + joint_state.name[i] = m_prefix + m_arm_joint_names[i]; joint_state.position[i] = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(base_feedback.actuators[i].position)); joint_state.velocity[i] = m_math_util.toRad(base_feedback.actuators[i].velocity); joint_state.effort[i] = base_feedback.actuators[i].torque; @@ -627,7 +593,7 @@ void KortexArmDriver::publishRobotFeedback() for (int i = 0; i < base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor.size(); i++) { int joint_state_index = base_feedback.actuators.size() + i; - joint_state.name[joint_state_index] = m_gripper_joint_names[i]; + joint_state.name[joint_state_index] = m_prefix + m_gripper_joint_names[i]; // Arm feedback is between 0 and 100, and limits in URDF are specified in gripper_joint_limits_min[i] and gripper_joint_limits_max[i] parameters joint_state.position[joint_state_index] = m_math_util.absolute_position_from_relative(base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor[i].position / 100.0, m_gripper_joint_limits_min[i], m_gripper_joint_limits_max[i]); joint_state.velocity[joint_state_index] = base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor[i].velocity; diff --git a/kortex_driver/src/non-generated/tests/main.cc b/kortex_driver/src/non-generated/tests/main.cc index d330136e..3a0312a6 100644 --- a/kortex_driver/src/non-generated/tests/main.cc +++ b/kortex_driver/src/non-generated/tests/main.cc @@ -8,6 +8,9 @@ int main(int argc, char** argv){ std::thread t([]{while(ros::ok()) ros::spin();}); + // Don't run simulator tests (the tests were mostly used during early development) + testing::GTEST_FLAG(filter) = "-KortexSimulatorTest.*"; + auto res = RUN_ALL_TESTS(); ros::shutdown(); diff --git a/kortex_driver/srv/generated/base/OnNotificationControlModeTopic.srv b/kortex_driver/srv/generated/base/Base_OnNotificationControlModeTopic.srv similarity index 100% rename from kortex_driver/srv/generated/base/OnNotificationControlModeTopic.srv rename to kortex_driver/srv/generated/base/Base_OnNotificationControlModeTopic.srv diff --git a/kortex_driver/srv/generated/base/ComputeForwardKinematics.srv b/kortex_driver/srv/generated/base/ComputeForwardKinematics.srv new file mode 100644 index 00000000..52a57709 --- /dev/null +++ b/kortex_driver/srv/generated/base/ComputeForwardKinematics.srv @@ -0,0 +1,3 @@ +JointAngles input +--- +Pose output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/ComputeInverseKinematics.srv b/kortex_driver/srv/generated/base/ComputeInverseKinematics.srv new file mode 100644 index 00000000..b0715d44 --- /dev/null +++ b/kortex_driver/srv/generated/base/ComputeInverseKinematics.srv @@ -0,0 +1,3 @@ +IKData input +--- +JointAngles output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/ExecuteWaypointTrajectory.srv b/kortex_driver/srv/generated/base/ExecuteWaypointTrajectory.srv new file mode 100644 index 00000000..8f837397 --- /dev/null +++ b/kortex_driver/srv/generated/base/ExecuteWaypointTrajectory.srv @@ -0,0 +1,3 @@ +WaypointList input +--- +Empty output \ No newline at end of file diff --git a/kortex_driver/srv/generated/base/ValidateWaypointList.srv b/kortex_driver/srv/generated/base/ValidateWaypointList.srv new file mode 100644 index 00000000..cd0e5c8c --- /dev/null +++ b/kortex_driver/srv/generated/base/ValidateWaypointList.srv @@ -0,0 +1,3 @@ +WaypointList input +--- +WaypointValidationReport output \ No newline at end of file diff --git a/kortex_driver/srv/generated/control_config/ControlConfig_OnNotificationControlModeTopic.srv b/kortex_driver/srv/generated/control_config/ControlConfig_OnNotificationControlModeTopic.srv new file mode 100644 index 00000000..43c15c71 --- /dev/null +++ b/kortex_driver/srv/generated/control_config/ControlConfig_OnNotificationControlModeTopic.srv @@ -0,0 +1,3 @@ +NotificationOptions input +--- +NotificationHandle output \ No newline at end of file diff --git a/kortex_driver/srv/generated/interconnect_config/GetGPIOConfiguration.srv b/kortex_driver/srv/generated/interconnect_config/GetGPIOConfiguration.srv index 4674efd1..a2fdd177 100644 --- a/kortex_driver/srv/generated/interconnect_config/GetGPIOConfiguration.srv +++ b/kortex_driver/srv/generated/interconnect_config/GetGPIOConfiguration.srv @@ -1,3 +1,3 @@ GPIOIdentification input --- -GPIOConfiguration output \ No newline at end of file +InterconnectConfig_GPIOConfiguration output \ No newline at end of file diff --git a/kortex_driver/srv/generated/interconnect_config/SetGPIOConfiguration.srv b/kortex_driver/srv/generated/interconnect_config/SetGPIOConfiguration.srv index 0cefe47e..f0349c12 100644 --- a/kortex_driver/srv/generated/interconnect_config/SetGPIOConfiguration.srv +++ b/kortex_driver/srv/generated/interconnect_config/SetGPIOConfiguration.srv @@ -1,3 +1,3 @@ -GPIOConfiguration input +InterconnectConfig_GPIOConfiguration input --- Empty output \ No newline at end of file diff --git a/kortex_driver/templates/ros_converter.cpp.jinja2 b/kortex_driver/templates/ros_converter.cpp.jinja2 index 538771dc..a3567247 100644 --- a/kortex_driver/templates/ros_converter.cpp.jinja2 +++ b/kortex_driver/templates/ros_converter.cpp.jinja2 @@ -26,7 +26,7 @@ int ToRosData({{detailed_message.cpp_namespace}}::{{detailed_message.name}} inpu output.{{field.name}}.clear(); for(int i = 0; i < input.{{field.name|lower}}_size(); i++) { - kortex_driver::{{field.type_name.split(".")[-1]}} temp; + decltype(output.{{field.name}})::value_type temp; ToRosData(input.{{field.name|lower}}(i), temp); output.{{field.name}}.push_back(temp); } diff --git a/kortex_driver/templates/services_interface.h.jinja2 b/kortex_driver/templates/services_interface.h.jinja2 index 9350d79f..346d2bd8 100644 --- a/kortex_driver/templates/services_interface.h.jinja2 +++ b/kortex_driver/templates/services_interface.h.jinja2 @@ -28,7 +28,7 @@ {%- for method in package.methods %} #include "kortex_driver/{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}.h" {%- if method.is_notification_rpc %} -#include "kortex_driver/{{method_prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification.h" +#include "kortex_driver/{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification.h" {%- endif %} {%- endfor %} @@ -49,7 +49,7 @@ class I{{package.short_name}}Services {%- for method in package.methods %} virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) = 0; {%- if method.is_notification_rpc %} - virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) = 0; + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) = 0; {%- endif %} {%- endfor %} diff --git a/kortex_driver/templates/services_robot.cpp.jinja2 b/kortex_driver/templates/services_robot.cpp.jinja2 index d559de06..3ffeb9f1 100644 --- a/kortex_driver/templates/services_robot.cpp.jinja2 +++ b/kortex_driver/templates/services_robot.cpp.jinja2 @@ -29,7 +29,7 @@ m_pub_Error = m_node_handle.advertise("kortex_error", 1000); {%- for method in package.methods -%} {%- if method.is_notification_rpc %} - m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); + m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); m_is_activated_{{method.name}} = false; {%- endif -%} {%- endfor %} @@ -125,7 +125,7 @@ bool {{package.short_name}}RobotServices::{{method.prepend_rpc_package_name}}{{m {%- if method.is_notification_rpc %} void {{package.short_name}}RobotServices::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) { - kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + kortex_driver::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification ros_msg; ToRosData(notif, ros_msg); m_pub_{{method.name}}.publish(ros_msg); } diff --git a/kortex_driver/templates/services_robot.h.jinja2 b/kortex_driver/templates/services_robot.h.jinja2 index 7a19498d..d980bace 100644 --- a/kortex_driver/templates/services_robot.h.jinja2 +++ b/kortex_driver/templates/services_robot.h.jinja2 @@ -34,7 +34,7 @@ class {{package.short_name}}RobotServices : public I{{package.short_name}}Servic {%- for method in package.methods %} virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; {%- if method.is_notification_rpc %} - virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) override; {%- endif %} {%- endfor %} diff --git a/kortex_driver/templates/services_simulation.cpp.jinja2 b/kortex_driver/templates/services_simulation.cpp.jinja2 index 6c478764..d1d86220 100644 --- a/kortex_driver/templates/services_simulation.cpp.jinja2 +++ b/kortex_driver/templates/services_simulation.cpp.jinja2 @@ -25,7 +25,7 @@ m_pub_Error = m_node_handle.advertise("kortex_error", 1000); {%- for method in package.methods -%} {%- if method.is_notification_rpc %} - m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); + m_pub_{{method.name}} = m_node_handle.advertise("{{method.name_lowercase_with_underscores}}", 1000); m_is_activated_{{method.name}} = false; {%- endif -%} {%- endfor %} @@ -73,7 +73,7 @@ bool {{package.short_name}}SimulationServices::{{method.prepend_rpc_package_name {%- if method.is_notification_rpc %} void {{package.short_name}}SimulationServices::cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) { - kortex_driver::{{method.name|replace("Topic", "")}}Notification ros_msg; + kortex_driver::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification ros_msg; ToRosData(notif, ros_msg); m_pub_{{method.name}}.publish(ros_msg); } diff --git a/kortex_driver/templates/services_simulation.h.jinja2 b/kortex_driver/templates/services_simulation.h.jinja2 index c2ccd970..60efd24c 100644 --- a/kortex_driver/templates/services_simulation.h.jinja2 +++ b/kortex_driver/templates/services_simulation.h.jinja2 @@ -32,7 +32,7 @@ class {{package.short_name}}SimulationServices : public I{{package.short_name}}S std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; {%- if method.is_notification_rpc %} - virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; + virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.name|replace("Topic", "")}}Notification notif) override; {%- endif %} {%- endfor %} diff --git a/kortex_examples/CMakeLists.txt b/kortex_examples/CMakeLists.txt index 378adbb7..c5c10d7c 100644 --- a/kortex_examples/CMakeLists.txt +++ b/kortex_examples/CMakeLists.txt @@ -6,7 +6,7 @@ add_compile_options(-std=c++11) add_definitions(-D_OS_UNIX) ## Find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation kortex_driver) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib_msgs actionlib kortex_driver) ## Declare a catkin package catkin_package() @@ -35,6 +35,10 @@ add_executable(example_cartesian_poses_with_notifications_cpp src/full_arm/examp add_dependencies(example_cartesian_poses_with_notifications_cpp ${catkin_EXPORTED_TARGETS}) target_link_libraries(example_cartesian_poses_with_notifications_cpp ${catkin_LIBRARIES} ) +add_executable(example_waypoint_action_client_cpp src/full_arm/example_waypoint_action_client.cpp) +add_dependencies(example_waypoint_action_client_cpp ${catkin_EXPORTED_TARGETS}) +target_link_libraries(example_waypoint_action_client_cpp ${catkin_LIBRARIES} ) + if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) catkin_add_gtest(kortex_examples_tests src/tests/kortex_examples_tests.cc) @@ -48,6 +52,7 @@ endif() catkin_install_python(PROGRAMS src/actuator_config/example_actuator_configuration.py src/full_arm/example_cartesian_poses_with_notifications.py src/full_arm/example_full_arm_movement.py + src/full_arm/example_waypoint_action_client.py src/move_it/example_move_it_trajectories.py src/vision_config/example_vision_configuration.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) @@ -58,6 +63,7 @@ install(TARGETS example_vision_configuration_cpp example_full_arm_movement_cpp example_cartesian_poses_with_notifications_cpp + example_waypoint_action_client_cpp ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/kortex_examples/launch/run_all_examples.launch b/kortex_examples/launch/run_all_examples.launch index d20e6edc..5335d0a1 100644 --- a/kortex_examples/launch/run_all_examples.launch +++ b/kortex_examples/launch/run_all_examples.launch @@ -44,12 +44,22 @@ + + + + + + + + + + - + - + diff --git a/kortex_examples/launch/waypoint_action_client_cpp.launch b/kortex_examples/launch/waypoint_action_client_cpp.launch new file mode 100644 index 00000000..538b5fa1 --- /dev/null +++ b/kortex_examples/launch/waypoint_action_client_cpp.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_examples/launch/waypoint_action_client_python.launch b/kortex_examples/launch/waypoint_action_client_python.launch new file mode 100644 index 00000000..83f31988 --- /dev/null +++ b/kortex_examples/launch/waypoint_action_client_python.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_examples/package.xml b/kortex_examples/package.xml index 6d9fbc58..9cdff593 100644 --- a/kortex_examples/package.xml +++ b/kortex_examples/package.xml @@ -14,6 +14,7 @@ roscpp rospy std_msgs + actionlib_msgs roscpp rospy std_msgs @@ -25,6 +26,8 @@ message_runtime moveit_commander moveit_commander + actionlib_msgs + message_generation diff --git a/kortex_examples/readme.md b/kortex_examples/readme.md index 2083281e..5936628b 100644 --- a/kortex_examples/readme.md +++ b/kortex_examples/readme.md @@ -70,9 +70,9 @@ There are a couple ways to use a Kortex arm with ROS, may it be in simulation or ![](./img/moveit.png) - **With a real arm**, the FollowJointTrajectory Action Server uses the Kortex API `PlayPreComputedJointTrajectory`. This call expects a full joint trajectory interpolated at a 1ms timestep, because the arm takes the trajectory as is, verifies it respects all velocity and acceleration constraints and then executes it as a low-level trajectory. Because of this timestep constraint, the MoveIt OMPL planning pipeline has an additional step which uses the `industrial_trajectory_filters/UniformSampleFilter` to interpolate the MoveIt output with a 1ms-timestep. Any timestep that provides a wrong velocity or acceleration causes the whole trajectory to be rejected. The velocity and acceleration limits in the configuration files have been tuned so no trajectory should yield such values, but if you experience trajectory rejection problems, you can tune down those parameters. + **With a real arm**, the FollowJointTrajectory Action Server uses the Kortex API `ExecuteWaypointTrajectory`. This call takes as input Angular Waypoints each with their own duration. Any waypoint that yields an invalid velocity or acceleration in its segment causes the whole trajectory to be rejected. The velocity and acceleration limits in the configuration files have been tuned so no trajectory should yield such values, but if you experience trajectory rejection problems, you can tune down those parameters. - **In simulation**, the FollowJointTrajectory and GripperCommand Action Servers are the ones spawned by the ros_controllers used with Gazebo. They don't need 1ms-timestep, but no option was added in MoveIt to switch between simulated or real action servers, so the same interpolator is used in simulation although it is not required. The Gen3 Intel Realsense camera is not simulated. + **In simulation**, the FollowJointTrajectory and GripperCommand Action Servers are the ones spawned by the ros_controllers used with Gazebo. The Gen3 Intel Realsense camera is not simulated. 3. Low-level control diff --git a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp index 37b09438..e9d7406f 100644 --- a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp +++ b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp @@ -60,9 +60,10 @@ bool wait_for_action_end_or_abort() } ros::spinOnce(); } + return false; } -bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) +bool example_home_the_robot(ros::NodeHandle n, const std::string &robot_name) { ros::ServiceClient service_client_read_action = n.serviceClient("/" + robot_name + "/base/read_action"); kortex_driver::ReadAction service_read_action; @@ -97,25 +98,8 @@ bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) return wait_for_action_end_or_abort(); } -bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) +bool example_cartesian_action(ros::NodeHandle n, const std::string &robot_name) { - ros::ServiceClient service_client_activate_notif = n.serviceClient("/" + robot_name + "/base/activate_publishing_of_action_topic"); - kortex_driver::OnNotificationActionTopic service_activate_notif; - - // We need to call this service to activate the Action Notification on the kortex_driver node. - if (service_client_activate_notif.call(service_activate_notif)) - { - ROS_INFO("Action notification activated!"); - } - else - { - std::string error_string = "Action notification publication failed"; - ROS_ERROR("%s", error_string.c_str()); - return false; - } - - ros::Duration(1.00).sleep(); - kortex_driver::ConstrainedPose my_constrained_pose; kortex_driver::CartesianSpeed my_cartesian_speed; @@ -206,7 +190,7 @@ bool example_cartesian_action(ros::NodeHandle n, std::string robot_name) } // This function sets the reference frame to the robot's base -bool example_set_cartesian_reference_frame(ros::NodeHandle n, std::string robot_name) +bool example_set_cartesian_reference_frame(ros::NodeHandle n, const std::string &robot_name) { // Initialize the ServiceClient ros::ServiceClient service_client_set_cartesian_reference_frame = n.serviceClient("/" + robot_name + "/control_config/set_cartesian_reference_frame"); @@ -256,7 +240,23 @@ int main(int argc, char **argv) ROS_INFO("%s", error_string.c_str()); } + // We need to call this service to activate the Action Notification on the kortex_driver node. + ros::ServiceClient service_client_activate_notif = n.serviceClient("/" + robot_name + "/base/activate_publishing_of_action_topic"); + kortex_driver::OnNotificationActionTopic service_activate_notif; + if (service_client_activate_notif.call(service_activate_notif)) + { + ROS_INFO("Action notification activated!"); + } + else + { + std::string error_string = "Action notification publication failed"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + ros::Duration(1.00).sleep(); ros::Subscriber sub = n.subscribe("/" + robot_name + "/action_topic", 1000, notification_callback); + + // Run the example success &= example_set_cartesian_reference_frame(n, robot_name); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); success &= example_home_the_robot(n, robot_name); diff --git a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py index 9e9e3dde..885c0fa9 100755 --- a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py +++ b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.py @@ -27,6 +27,8 @@ def __init__(self): self.action_topic_sub = None self.all_notifs_succeeded = True + self.all_notifs_succeeded = True + # Get node params self.robot_name = rospy.get_param('~robot_name', "my_gen3") @@ -159,11 +161,6 @@ def main(self): # Make sure to clear the robot's faults else it won't move if it's already in fault success &= self.example_clear_faults() #******************************************************************************* - - #******************************************************************************* - # Subscribe to ActionNotification's from the robot to know when a cartesian pose is finished - success &= self.example_subscribe_to_a_robot_notification() - #******************************************************************************* #******************************************************************************* # Start the example from the Home position @@ -174,6 +171,10 @@ def main(self): # Set the reference frame to "Mixed" success &= self.example_set_cartesian_reference_frame() + #******************************************************************************* + # Subscribe to ActionNotification's from the robot to know when a cartesian pose is finished + success &= self.example_subscribe_to_a_robot_notification() + #******************************************************************************* # Prepare and send pose 1 my_cartesian_speed = CartesianSpeed() @@ -250,6 +251,8 @@ def main(self): success &= self.all_notifs_succeeded + success &= self.all_notifs_succeeded + # For testing purposes rospy.set_param("/kortex_examples_test_results/cartesian_poses_with_notifications_python", success) diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.cpp b/kortex_examples/src/full_arm/example_full_arm_movement.cpp index 8459f123..edcd474b 100644 --- a/kortex_examples/src/full_arm/example_full_arm_movement.cpp +++ b/kortex_examples/src/full_arm/example_full_arm_movement.cpp @@ -13,6 +13,7 @@ #include #include "ros/ros.h" + #include #include #include @@ -27,6 +28,9 @@ #include #include #include +#include +#include +#include #define HOME_ACTION_IDENTIFIER 2 @@ -53,11 +57,29 @@ bool wait_for_action_end_or_abort() } ros::spinOnce(); } + return false; } +kortex_driver::Waypoint FillCartesianWaypoint(float new_x, float new_y, float new_z, float new_theta_x, float new_theta_y, float new_theta_z, float blending_radius) +{ + kortex_driver::Waypoint waypoint; + kortex_driver::CartesianWaypoint cartesianWaypoint; + + cartesianWaypoint.pose.x = new_x; + cartesianWaypoint.pose.y = new_y; + cartesianWaypoint.pose.z = new_z; + cartesianWaypoint.pose.theta_x = new_theta_x; + cartesianWaypoint.pose.theta_y = new_theta_y; + cartesianWaypoint.pose.theta_z = new_theta_z; + cartesianWaypoint.reference_frame = kortex_driver::CartesianReferenceFrame::CARTESIAN_REFERENCE_FRAME_BASE; + cartesianWaypoint.blending_radius = blending_radius; + waypoint.oneof_type_of_waypoint.cartesian_waypoint.push_back(cartesianWaypoint); -bool example_clear_faults(ros::NodeHandle n, std::string robot_name) + return waypoint; +} + +bool example_clear_faults(ros::NodeHandle n, const std::string &robot_name) { ros::ServiceClient service_client_clear_faults = n.serviceClient("/" + robot_name + "/base/clear_faults"); kortex_driver::Base_ClearFaults service_clear_faults; @@ -75,7 +97,7 @@ bool example_clear_faults(ros::NodeHandle n, std::string robot_name) return true; } -bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) +bool example_home_the_robot(ros::NodeHandle n, const std::string &robot_name) { ros::ServiceClient service_client_read_action = n.serviceClient("/" + robot_name + "/base/read_action"); kortex_driver::ReadAction service_read_action; @@ -111,7 +133,7 @@ bool example_home_the_robot(ros::NodeHandle n, std::string robot_name) return wait_for_action_end_or_abort(); } -bool example_set_cartesian_reference_frame(ros::NodeHandle n, std::string robot_name) +bool example_set_cartesian_reference_frame(ros::NodeHandle n, const std::string &robot_name) { // Initialize the ServiceClient ros::ServiceClient service_client_set_cartesian_reference_frame = n.serviceClient("/" + robot_name + "/control_config/set_cartesian_reference_frame"); @@ -131,7 +153,7 @@ bool example_set_cartesian_reference_frame(ros::NodeHandle n, std::string robot_ return true; } -bool example_send_cartesian_pose(ros::NodeHandle n, std::string robot_name) +bool example_send_cartesian_pose(ros::NodeHandle n, const std::string &robot_name) { last_action_notification_event = 0; // Get the actual cartesian pose to increment it @@ -181,7 +203,7 @@ bool example_send_cartesian_pose(ros::NodeHandle n, std::string robot_name) return wait_for_action_end_or_abort(); } -bool example_send_joint_angles(ros::NodeHandle n, std::string robot_name, int degrees_of_freedom) +bool example_send_joint_angles(ros::NodeHandle n, const std::string &robot_name, int degrees_of_freedom) { last_action_notification_event = 0; // Initialize the ServiceClient @@ -216,7 +238,7 @@ bool example_send_joint_angles(ros::NodeHandle n, std::string robot_name, int de return wait_for_action_end_or_abort(); } -bool example_send_gripper_command(ros::NodeHandle n, std::string robot_name, double value) +bool example_send_gripper_command(ros::NodeHandle n, const std::string &robot_name, double value) { // Initialize the ServiceClient ros::ServiceClient service_client_send_gripper_command = n.serviceClient("/" + robot_name + "/base/send_gripper_command"); @@ -243,6 +265,57 @@ bool example_send_gripper_command(ros::NodeHandle n, std::string robot_name, dou return true; } +bool example_cartesian_waypoint(ros::NodeHandle n, const std::string &robot_name) +{ + + ros::ServiceClient service_client_execute_waypoints_trajectory = n.serviceClient("/" + robot_name + "/base/execute_waypoint_trajectory"); + ros::ServiceClient service_client_get_config = n.serviceClient("/" + robot_name + "/base/get_product_configuration"); + + kortex_driver::ExecuteWaypointTrajectory service_execute_waypoints_trajectory; + kortex_driver::GetProductConfiguration service_get_config; + + last_action_notification_event = 0; + + if (!service_client_get_config.call(service_get_config)) + { + std::string error_string = "Failed to call GetProductConfiguration"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + auto product_config = service_get_config.response.output; + + if(product_config.model == kortex_driver::ModelId::MODEL_ID_L31) //If the robot is a GEN3-LITE use this trajectory. + { + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.439, 0.194, 0.448, 90.6, -1.0, 150, 0)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.200, 0.150, 0.400, 90.6, -1.0, 150, 0)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.350, 0.050, 0.300, 90.6, -1.0, 150, 0)); + } + else + { + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.7, 0.0, 0.5, 90, 0, 90, 0)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.7, 0.0, 0.33, 90, 0, 90, 0.1)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.7, 0.48, 0.33, 90, 0, 90, 0.1)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.61, 0.22, 0.4, 90, 0, 90, 0.1)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.7, 0.48, 0.33, 90, 0, 90, 0.1)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.63, -0.22, 0.45, 90, 0, 90, 0.1)); + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(0.65, 0.05, 0.45, 90, 0, 90, 0)); + } + + + service_execute_waypoints_trajectory.request.input.duration = 0; + service_execute_waypoints_trajectory.request.input.use_optimal_blending = 0; + + if (!service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) + { + std::string error_string = "Failed to call ExecuteWaypointTrajectory"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + return wait_for_action_end_or_abort(); +} + int main(int argc, char **argv) { ros::init(argc, argv, "full_arm_movement_example_cpp"); @@ -255,8 +328,11 @@ int main(int argc, char **argv) //******************************************************************************* // ROS Parameters ros::NodeHandle n; + std::string robot_name = "my_gen3"; + int degrees_of_freedom = 7; + bool is_gripper_present = false; // Parameter robot_name @@ -356,6 +432,21 @@ int main(int argc, char **argv) } //******************************************************************************* + //******************************************************************************* + // Move the robot to the Home position again with an Action + success &= example_home_the_robot(n, robot_name); + //******************************************************************************* + + //******************************************************************************* + // Move the robot using Cartesian waypoint. + success &= example_cartesian_waypoint(n, robot_name); + //******************************************************************************* + + //******************************************************************************* + // Move the robot to the Home position one last time. + success &= example_home_the_robot(n, robot_name); + //******************************************************************************* + // Report success for testing purposes ros::param::set("/kortex_examples_test_results/full_arm_movement_cpp", success); diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.py b/kortex_examples/src/full_arm/example_full_arm_movement.py index 14868b1f..829b9c3a 100755 --- a/kortex_examples/src/full_arm/example_full_arm_movement.py +++ b/kortex_examples/src/full_arm/example_full_arm_movement.py @@ -14,9 +14,11 @@ import sys import rospy import time + from kortex_driver.srv import * from kortex_driver.msg import * + class ExampleFullArmMovement: def __init__(self): try: @@ -67,6 +69,10 @@ def __init__(self): activate_publishing_of_action_notification_full_name = '/' + self.robot_name + '/base/activate_publishing_of_action_topic' rospy.wait_for_service(activate_publishing_of_action_notification_full_name) self.activate_publishing_of_action_notification = rospy.ServiceProxy(activate_publishing_of_action_notification_full_name, OnNotificationActionTopic) + + get_product_configuration_full_name = '/' + self.robot_name + '/base/get_product_configuration' + rospy.wait_for_service(get_product_configuration_full_name) + self.get_product_configuration = rospy.ServiceProxy(get_product_configuration_full_name, GetProductConfiguration) except: self.is_init_success = False else: @@ -74,6 +80,22 @@ def __init__(self): def cb_action_topic(self, notif): self.last_action_notif_type = notif.action_event + + def FillCartesianWaypoint(self, new_x, new_y, new_z, new_theta_x, new_theta_y, new_theta_z, blending_radius): + waypoint = Waypoint() + cartesianWaypoint = CartesianWaypoint() + + cartesianWaypoint.pose.x = new_x + cartesianWaypoint.pose.y = new_y + cartesianWaypoint.pose.z = new_z + cartesianWaypoint.pose.theta_x = new_theta_x + cartesianWaypoint.pose.theta_y = new_theta_y + cartesianWaypoint.pose.theta_z = new_theta_z + cartesianWaypoint.reference_frame = CartesianReferenceFrame.CARTESIAN_REFERENCE_FRAME_BASE + cartesianWaypoint.blending_radius = blending_radius + waypoint.oneof_type_of_waypoint.cartesian_waypoint.append(cartesianWaypoint) + + return waypoint def wait_for_action_end_or_abort(self): while not rospy.is_shutdown(): @@ -231,6 +253,40 @@ def example_send_gripper_command(self, value): time.sleep(0.5) return True + def example_cartesian_waypoint_action(self): + self.last_action_notif_type = None + + req = ExecuteActionRequest() + trajectory = WaypointList() + + config = self.get_product_configuration() + + if config.output.model == ModelId.MODEL_ID_L31: + + trajectory.waypoints.append(self.FillCartesianWaypoint(0.439, 0.194, 0.448, 90.6, -1.0, 150, 0)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.200, 0.150, 0.400, 90.6, -1.0, 150, 0)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.350, 0.050, 0.300, 90.6, -1.0, 150, 0)) + else: + trajectory.waypoints.append(self.FillCartesianWaypoint(0.7, 0.0, 0.5, 90, 0, 90, 0)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.7, 0.0, 0.33, 90, 0, 90, 0.1)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.7, 0.48, 0.33, 90, 0, 90, 0.1)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.61, 0.22, 0.4, 90, 0, 90, 0.1)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.7, 0.48, 0.33, 90, 0, 90, 0.1)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.63, -0.22, 0.45, 90, 0, 90, 0.1)) + trajectory.waypoints.append(self.FillCartesianWaypoint(0.65, 0.05, 0.45, 90, 0, 90, 0)) + + req.input.oneof_action_parameters.execute_waypoint_list.append(trajectory) + + # Call the service + rospy.loginfo("Executing Kortex action ExecuteWaypointTrajectory...") + try: + self.execute_action(req) + except rospy.ServiceException: + rospy.logerr("Failed to call action ExecuteWaypointTrajectory") + return False + else: + return self.wait_for_action_end_or_abort() + def main(self): # For testing purposes success = self.is_init_success @@ -288,6 +344,21 @@ def main(self): rospy.logwarn("No gripper is present on the arm.") #******************************************************************************* + #******************************************************************************* + # Move the robot to the Home position with an Action + success &= self.example_home_the_robot() + #******************************************************************************* + + #******************************************************************************* + # Example of waypoint + # Let's move the arm + success &= self.example_cartesian_waypoint_action() + + #******************************************************************************* + # Move the robot to the Home position with an Action + success &= self.example_home_the_robot() + #******************************************************************************* + # For testing purposes rospy.set_param("/kortex_examples_test_results/full_arm_movement_python", success) diff --git a/kortex_examples/src/full_arm/example_waypoint_action_client.cpp b/kortex_examples/src/full_arm/example_waypoint_action_client.cpp new file mode 100644 index 00000000..3383631e --- /dev/null +++ b/kortex_examples/src/full_arm/example_waypoint_action_client.cpp @@ -0,0 +1,285 @@ +/* + * KINOVA (R) KORTEX (TM) + * + * Copyright (c) 2021 Kinova inc. All rights reserved. + * + * This software may be modified and distributed + * under the terms of the BSD 3-Clause license. + * + * Refer to the LICENSE file for details. + * + */ + +#include +#include +#include + +#include +#include + +#include "actionlib/client/simple_action_client.h" +#include "kortex_driver/Base_ClearFaults.h" +#include "kortex_driver/BaseCyclic_Feedback.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/ActionEvent.h" +#include "kortex_driver/OnNotificationActionTopic.h" +#include +#include "kortex_driver/FollowCartesianTrajectoryAction.h" +#include "kortex_driver/FollowCartesianTrajectoryActionFeedback.h" +#include "kortex_driver/FollowCartesianTrajectoryActionGoal.h" +#include "kortex_driver/FollowCartesianTrajectoryActionResult.h" +#include +#include + + +#define HOME_ACTION_IDENTIFIER 2 + +std::atomic last_action_notification_event{0}; + +void notification_callback(const kortex_driver::ActionNotification& notif) +{ + last_action_notification_event = notif.action_event; +} + +bool wait_for_action_end_or_abort() +{ + while (ros::ok()) + { + if (last_action_notification_event.load() == kortex_driver::ActionEvent::ACTION_END) + { + ROS_INFO("Received ACTION_END notification"); + return true; + } + else if (last_action_notification_event.load() == kortex_driver::ActionEvent::ACTION_ABORT) + { + ROS_INFO("Received ACTION_ABORT notification"); + return false; + } + ros::spinOnce(); + } + return false; +} + +kortex_driver::CartesianWaypoint FillCartesianWaypoint(float new_x, float new_y, float new_z, float new_theta_x, float new_theta_y, float new_theta_z, float blending_radius) +{ + kortex_driver::CartesianWaypoint cartesianWaypoint; + + cartesianWaypoint.pose.x = new_x; + cartesianWaypoint.pose.y = new_y; + cartesianWaypoint.pose.z = new_z; + cartesianWaypoint.pose.theta_x = new_theta_x; + cartesianWaypoint.pose.theta_y = new_theta_y; + cartesianWaypoint.pose.theta_z = new_theta_z; + cartesianWaypoint.reference_frame = kortex_driver::CartesianReferenceFrame::CARTESIAN_REFERENCE_FRAME_BASE; + cartesianWaypoint.blending_radius = blending_radius; + + return cartesianWaypoint; +} + +bool example_clear_faults(ros::NodeHandle n, const std::string &robot_name) +{ + ros::ServiceClient service_client_clear_faults = n.serviceClient("/" + robot_name + "/base/clear_faults"); + kortex_driver::Base_ClearFaults service_clear_faults; + + // Clear the faults + if (!service_client_clear_faults.call(service_clear_faults)) + { + std::string error_string = "Failed to clear the faults"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + // Wait a bit + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + return true; +} + +bool example_home_the_robot(ros::NodeHandle n, const std::string &robot_name) +{ + ros::ServiceClient service_client_read_action = n.serviceClient("/" + robot_name + "/base/read_action"); + kortex_driver::ReadAction service_read_action; + last_action_notification_event = 0; + + // The Home Action is used to home the robot. It cannot be deleted and is always ID #2: + service_read_action.request.input.identifier = HOME_ACTION_IDENTIFIER; + + if (!service_client_read_action.call(service_read_action)) + { + std::string error_string = "Failed to call ReadAction"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + // We can now execute the Action that we read + ros::ServiceClient service_client_execute_action = n.serviceClient("/" + robot_name + "/base/execute_action"); + kortex_driver::ExecuteAction service_execute_action; + + service_execute_action.request.input = service_read_action.response.output; + + if (service_client_execute_action.call(service_execute_action)) + { + ROS_INFO("The Home position action was sent to the robot."); + } + else + { + std::string error_string = "Failed to call ExecuteAction"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + return wait_for_action_end_or_abort(); +} + +bool example_cartesian_waypoint_action(ros::NodeHandle n, const std::string &robot_name) +{ + ros::ServiceClient service_client_get_config = n.serviceClient("/" + robot_name + "/base/get_product_configuration"); + kortex_driver::GetProductConfiguration service_get_config; + + actionlib::SimpleActionClient ac(robot_name + "/cartesian_trajectory_controller/follow_cartesian_trajectory", true); + ros::Duration server_timeout(5, 0); + + ROS_INFO("Waiting for the server."); + + // wait for the action server to start + ac.waitForServer(); + + ROS_INFO("Server responded."); + + kortex_driver::FollowCartesianTrajectoryGoal goal; + + goal.use_optimal_blending = false; + + if (!service_client_get_config.call(service_get_config)) + { + std::string error_string = "Failed to call GetProductConfiguration"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + auto product_config = service_get_config.response.output; + + if(product_config.model == kortex_driver::ModelId::MODEL_ID_L31) //If the robot is a GEN3-LITE use this trajectory. + { + goal.trajectory.push_back(FillCartesianWaypoint(0.439, 0.194, 0.448, angles::from_degrees(90.6), angles::from_degrees(-1.0), angles::from_degrees(150), 0)); + goal.trajectory.push_back(FillCartesianWaypoint(0.200, 0.150, 0.400, angles::from_degrees(90.6), angles::from_degrees(-1.0), angles::from_degrees(150), 0)); + goal.trajectory.push_back(FillCartesianWaypoint(0.350, 0.050, 0.300, angles::from_degrees(90.6), angles::from_degrees(-1.0), angles::from_degrees(150), 0)); + } + else + { + goal.trajectory.push_back(FillCartesianWaypoint(0.7, 0.0, 0.5, angles::from_degrees(90), 0, angles::from_degrees(90), 0)); + goal.trajectory.push_back(FillCartesianWaypoint(0.7, 0.0, 0.33, angles::from_degrees(90), 0, angles::from_degrees(90), 0.1)); + goal.trajectory.push_back(FillCartesianWaypoint(0.7, 0.48, 0.33, angles::from_degrees(90), 0, angles::from_degrees(90), 0.1)); + goal.trajectory.push_back(FillCartesianWaypoint(0.61, 0.22, 0.4, angles::from_degrees(90), 0, angles::from_degrees(90), 0.1)); + goal.trajectory.push_back(FillCartesianWaypoint(0.7, 0.48, 0.33, angles::from_degrees(90), 0, angles::from_degrees(90), 0.1)); + goal.trajectory.push_back(FillCartesianWaypoint(0.63, -0.22, 0.45, angles::from_degrees(90), 0, angles::from_degrees(90), 0.1)); + goal.trajectory.push_back(FillCartesianWaypoint(0.65, 0.05, 0.45, angles::from_degrees(90), 0, angles::from_degrees(90), 0)); + } + + ac.sendGoal(goal); + + //wait for the action to return + bool completed = ac.waitForResult(ros::Duration(50.0)); + + if (completed) + { + actionlib::SimpleClientGoalState state = ac.getState(); + ROS_INFO("Action finished: %s",state.toString().c_str()); + } + else + { + ROS_ERROR("Action did not finish before the time out."); + } + + //exit + return completed; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "waypoint_action_cpp"); + + // For testing purpose + ros::param::del("/kortex_examples_test_results/waypoint_action_cpp"); + + bool success = true; + + //******************************************************************************* + // ROS Parameters + ros::NodeHandle n; + + //Here, put the name of your robot + std::string robot_name = "my_gen3"; + + //Here you specify your robot's degree of freedom + int degrees_of_freedom = 7; + + // Parameter robot_name + if (!ros::param::get("~robot_name", robot_name)) + { + std::string error_string = "Parameter robot_name was not specified, defaulting to " + robot_name + " as namespace"; + ROS_WARN("%s", error_string.c_str()); + } + else + { + std::string error_string = "Using robot_name " + robot_name + " as namespace"; + ROS_INFO("%s", error_string.c_str()); + } + + // Parameter degrees_of_freedom + if (!ros::param::get("/" + robot_name + "/degrees_of_freedom", degrees_of_freedom)) + { + std::string error_string = "Parameter /" + robot_name + "/degrees_of_freedom was not specified, defaulting to " + std::to_string(degrees_of_freedom) + " as degrees of freedom"; + ROS_WARN("%s", error_string.c_str()); + } + else + { + std::string error_string = "Using degrees_of_freedom " + std::to_string(degrees_of_freedom) + " as degrees_of_freedom"; + ROS_INFO("%s", error_string.c_str()); + } + + //******************************************************************************* + + // Subscribe to the Action Topic + ros::Subscriber sub = n.subscribe("/" + robot_name + "/action_topic", 1000, notification_callback); + + // We need to call this service to activate the Action Notification on the kortex_driver node. + ros::ServiceClient service_client_activate_notif = n.serviceClient("/" + robot_name + "/base/activate_publishing_of_action_topic"); + kortex_driver::OnNotificationActionTopic service_activate_notif; + if (service_client_activate_notif.call(service_activate_notif)) + { + ROS_INFO("Action notification activated!"); + } + else + { + std::string error_string = "Action notification publication failed"; + ROS_ERROR("%s", error_string.c_str()); + success = false; + } + + //******************************************************************************* + // Make sure to clear the robot's faults else it won't move if it's already in fault + success &= example_clear_faults(n, robot_name); + //******************************************************************************* + + //******************************************************************************* + // Move the robot to the Home position with an Action + success &= example_home_the_robot(n, robot_name); + //******************************************************************************* + + //******************************************************************************* + // Move the robot using Cartesian waypoint with the action server. + success &= example_cartesian_waypoint_action(n, robot_name); + //******************************************************************************* + + //******************************************************************************* + // Move the robot to the Home position one last time. + success &= example_home_the_robot(n, robot_name); + //******************************************************************************* + + // Report success for testing purposes + ros::param::set("/kortex_examples_test_results/waypoint_action_cpp", success); + + return success ? 0 : 1; +} \ No newline at end of file diff --git a/kortex_examples/src/full_arm/example_waypoint_action_client.py b/kortex_examples/src/full_arm/example_waypoint_action_client.py new file mode 100755 index 00000000..4f79ce9f --- /dev/null +++ b/kortex_examples/src/full_arm/example_waypoint_action_client.py @@ -0,0 +1,225 @@ +#!/usr/bin/env python +### +# KINOVA (R) KORTEX (TM) +# +# Copyright (c) 2021 Kinova inc. All rights reserved. +# +# This software may be modified and distributed +# under the terms of the BSD 3-Clause license. +# +# Refer to the LICENSE file for details. +# +### + +import sys +import rospy +import time +import math + +import actionlib + +from kortex_driver.srv import * +from kortex_driver.msg import * + +class ExampleWaypointActionClient: + def __init__(self): + try: + rospy.init_node('example_waypoint_action_python') + + self.HOME_ACTION_IDENTIFIER = 2 + + # Get node params + self.robot_name = rospy.get_param('~robot_name', "my_gen3") + self.degrees_of_freedom = rospy.get_param("/" + self.robot_name + "/degrees_of_freedom", 7) + self.is_gripper_present = rospy.get_param("/" + self.robot_name + "/is_gripper_present", False) + + rospy.loginfo("Using robot_name " + self.robot_name + " , robot has " + str(self.degrees_of_freedom) + " degrees of freedom and is_gripper_present is " + str(self.is_gripper_present)) + + # Init the action topic subscriber + self.action_topic_sub = rospy.Subscriber("/" + self.robot_name + "/action_topic", ActionNotification, self.cb_action_topic) + self.last_action_notif_type = None + + # Init the services + clear_faults_full_name = '/' + self.robot_name + '/base/clear_faults' + rospy.wait_for_service(clear_faults_full_name) + self.clear_faults = rospy.ServiceProxy(clear_faults_full_name, Base_ClearFaults) + + read_action_full_name = '/' + self.robot_name + '/base/read_action' + rospy.wait_for_service(read_action_full_name) + self.read_action = rospy.ServiceProxy(read_action_full_name, ReadAction) + + execute_action_full_name = '/' + self.robot_name + '/base/execute_action' + rospy.wait_for_service(execute_action_full_name) + self.execute_action = rospy.ServiceProxy(execute_action_full_name, ExecuteAction) + + activate_publishing_of_action_notification_full_name = '/' + self.robot_name + '/base/activate_publishing_of_action_topic' + rospy.wait_for_service(activate_publishing_of_action_notification_full_name) + self.activate_publishing_of_action_notification = rospy.ServiceProxy(activate_publishing_of_action_notification_full_name, OnNotificationActionTopic) + + get_product_configuration_full_name = '/' + self.robot_name + '/base/get_product_configuration' + rospy.wait_for_service(get_product_configuration_full_name) + self.get_product_configuration = rospy.ServiceProxy(get_product_configuration_full_name, GetProductConfiguration) + except: + self.is_init_success = False + else: + self.is_init_success = True + + def cb_action_topic(self, notif): + self.last_action_notif_type = notif.action_event + + def wait_for_action_end_or_abort(self): + while not rospy.is_shutdown(): + if (self.last_action_notif_type == ActionEvent.ACTION_END): + rospy.loginfo("Received ACTION_END notification") + return True + elif (self.last_action_notif_type == ActionEvent.ACTION_ABORT): + rospy.loginfo("Received ACTION_ABORT notification") + return False + else: + time.sleep(0.01) + + def FillCartesianWaypoint(self, new_x, new_y, new_z, new_theta_x, new_theta_y, new_theta_z, blending_radius): + cartesianWaypoint = CartesianWaypoint() + + cartesianWaypoint.pose.x = new_x + cartesianWaypoint.pose.y = new_y + cartesianWaypoint.pose.z = new_z + cartesianWaypoint.pose.theta_x = new_theta_x + cartesianWaypoint.pose.theta_y = new_theta_y + cartesianWaypoint.pose.theta_z = new_theta_z + cartesianWaypoint.reference_frame = CartesianReferenceFrame.CARTESIAN_REFERENCE_FRAME_BASE + cartesianWaypoint.blending_radius = blending_radius + + return cartesianWaypoint + + def example_subscribe_to_a_robot_notification(self): + # Activate the publishing of the ActionNotification + req = OnNotificationActionTopicRequest() + rospy.loginfo("Activating the action notifications...") + try: + self.activate_publishing_of_action_notification(req) + except rospy.ServiceException: + rospy.logerr("Failed to call OnNotificationActionTopic") + return False + else: + rospy.loginfo("Successfully activated the Action Notifications!") + + rospy.sleep(1.0) + return True + + def example_clear_faults(self): + try: + self.clear_faults() + except rospy.ServiceException: + rospy.logerr("Failed to call ClearFaults") + return False + else: + rospy.loginfo("Cleared the faults successfully") + rospy.sleep(2.5) + return True + + def example_home_the_robot(self): + # The Home Action is used to home the robot. It cannot be deleted and is always ID #2: + self.last_action_notif_type = None + req = ReadActionRequest() + req.input.identifier = self.HOME_ACTION_IDENTIFIER + try: + res = self.read_action(req) + except rospy.ServiceException: + rospy.logerr("Failed to call ReadAction") + return False + # Execute the HOME action if we could read it + else: + # What we just read is the input of the ExecuteAction service + req = ExecuteActionRequest() + req.input = res.output + rospy.loginfo("Sending the robot home...") + try: + self.execute_action(req) + except rospy.ServiceException: + rospy.logerr("Failed to call ExecuteAction") + return False + else: + return self.wait_for_action_end_or_abort() + + def example_cartesian_waypoint_action(self): + self.last_action_notif_type = None + + client = actionlib.SimpleActionClient('/' + self.robot_name + '/cartesian_trajectory_controller/follow_cartesian_trajectory', kortex_driver.msg.FollowCartesianTrajectoryAction) + + client.wait_for_server() + + goal = FollowCartesianTrajectoryGoal() + + config = self.get_product_configuration() + + if config.output.model == ModelId.MODEL_ID_L31: + + goal.trajectory.append(self.FillCartesianWaypoint(0.439, 0.194, 0.448, math.radians(90.6), math.radians(-1.0), math.radians(150), 0)) + goal.trajectory.append(self.FillCartesianWaypoint(0.200, 0.150, 0.400, math.radians(90.6), math.radians(-1.0), math.radians(150), 0)) + goal.trajectory.append(self.FillCartesianWaypoint(0.350, 0.050, 0.300, math.radians(90.6), math.radians(-1.0), math.radians(150), 0)) + else: + goal.trajectory.append(self.FillCartesianWaypoint(0.7, 0.0, 0.5, math.radians(90), 0, math.radians(90), 0)) + goal.trajectory.append(self.FillCartesianWaypoint(0.7, 0.0, 0.33, math.radians(90), 0, math.radians(90), 0.1)) + goal.trajectory.append(self.FillCartesianWaypoint(0.7, 0.48, 0.33, math.radians(90), 0, math.radians(90), 0.1)) + goal.trajectory.append(self.FillCartesianWaypoint(0.61, 0.22, 0.4, math.radians(90), 0, math.radians(90), 0.1)) + goal.trajectory.append(self.FillCartesianWaypoint(0.7, 0.48, 0.33, math.radians(90), 0, math.radians(90), 0.1)) + goal.trajectory.append(self.FillCartesianWaypoint(0.63, -0.22, 0.45, math.radians(90), 0, math.radians(90), 0.1)) + goal.trajectory.append(self.FillCartesianWaypoint(0.65, 0.05, 0.45, math.radians(90), 0, math.radians(90), 0)) + + # Call the service + rospy.loginfo("Sending goal(Cartesian waypoint) to action server...") + try: + client.send_goal(goal) + except rospy.ServiceException: + rospy.logerr("Failed to send goal.") + return False + else: + client.wait_for_result() + return True + + + def main(self): + # For testing purposes + success = self.is_init_success + try: + rospy.delete_param("/kortex_examples_test_results/waypoint_action_python") + except: + pass + + if success: + #******************************************************************************* + # Make sure to clear the robot's faults else it won't move if it's already in fault + success &= self.example_clear_faults() + #******************************************************************************* + + #******************************************************************************* + # Activate the action notifications + success &= self.example_subscribe_to_a_robot_notification() + #******************************************************************************* + + #******************************************************************************* + # Move the robot to the Home position with an Action + success &= self.example_home_the_robot() + #******************************************************************************* + + #******************************************************************************* + # Example of Cartesian waypoint using an action client + success &= self.example_cartesian_waypoint_action() + #******************************************************************************* + + #******************************************************************************* + # Move back the robot to the Home position with an Action + success &= self.example_home_the_robot() + #******************************************************************************* + + # For testing purposes + rospy.set_param("/kortex_examples_test_results/waypoint_action_python", success) + + if not success: + rospy.logerr("The example encountered an error.") + + +if __name__ == "__main__": + ex = ExampleWaypointActionClient() + ex.main() diff --git a/kortex_examples/src/move_it/example_move_it_trajectories.py b/kortex_examples/src/move_it/example_move_it_trajectories.py index 82db88f9..4d88a98c 100755 --- a/kortex_examples/src/move_it/example_move_it_trajectories.py +++ b/kortex_examples/src/move_it/example_move_it_trajectories.py @@ -191,50 +191,56 @@ def main(): if success: rospy.loginfo("Reaching Named Target Vertical...") - success &= example.reach_named_position("vertical") - - rospy.loginfo("Reaching Joint Angles...") - + print (success) + + if success: + rospy.loginfo("Reaching Joint Angles...") success &= example.reach_joint_angles(tolerance=0.01) #rad - + print (success) + + if success: rospy.loginfo("Reaching Named Target Home...") - success &= example.reach_named_position("home") + print (success) + if success: rospy.loginfo("Reaching Cartesian Pose...") actual_pose = example.get_cartesian_pose() actual_pose.position.z -= 0.2 success &= example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=None) + print (success) - if example.degrees_of_freedom == 7: - rospy.loginfo("Reach Cartesian Pose with constraints...") - # Get actual pose - actual_pose = example.get_cartesian_pose() - actual_pose.position.y -= 0.3 - - # Orientation constraint (we want the end effector to stay the same orientation) - constraints = moveit_msgs.msg.Constraints() - orientation_constraint = moveit_msgs.msg.OrientationConstraint() - orientation_constraint.orientation = actual_pose.orientation - constraints.orientation_constraints.append(orientation_constraint) - - # Send the goal - success &= example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=constraints) - - if example.is_gripper_present: - rospy.loginfo("Opening the gripper...") - success &= example.reach_gripper_position(0) - - rospy.loginfo("Closing the gripper 50%...") - success &= example.reach_gripper_position(0.5) - - # For testing purposes - rospy.set_param("/kortex_examples_test_results/moveit_general_python", success) - - if not success: - rospy.logerr("The example encountered an error.") + if example.degrees_of_freedom == 7 and success: + rospy.loginfo("Reach Cartesian Pose with constraints...") + # Get actual pose + actual_pose = example.get_cartesian_pose() + actual_pose.position.y -= 0.3 + + # Orientation constraint (we want the end effector to stay the same orientation) + constraints = moveit_msgs.msg.Constraints() + orientation_constraint = moveit_msgs.msg.OrientationConstraint() + orientation_constraint.orientation = actual_pose.orientation + constraints.orientation_constraints.append(orientation_constraint) + + # Send the goal + success &= example.reach_cartesian_pose(pose=actual_pose, tolerance=0.01, constraints=constraints) + + if example.is_gripper_present and success: + rospy.loginfo("Opening the gripper...") + success &= example.reach_gripper_position(0) + print (success) + + rospy.loginfo("Closing the gripper 50%...") + success &= example.reach_gripper_position(0.5) + print (success) + + # For testing purposes + rospy.set_param("/kortex_examples_test_results/moveit_general_python", success) + + if not success: + rospy.logerr("The example encountered an error.") if __name__ == '__main__': main() diff --git a/kortex_examples/src/tests/kortex_examples_tests.cc b/kortex_examples/src/tests/kortex_examples_tests.cc index e5cc8683..49e7071c 100644 --- a/kortex_examples/src/tests/kortex_examples_tests.cc +++ b/kortex_examples/src/tests/kortex_examples_tests.cc @@ -44,6 +44,8 @@ class KortexDriverTest : public ::testing::Test { {"full_arm_movement_cpp", true}, {"full_arm_movement_python", true}, {"actuator_configuration_cpp", true}, + {"waypoint_action_cpp", true}, + {"waypoint_action_python", true}, {"moveit_general_python", true} }; diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/default_joint_limits.yaml similarity index 77% rename from kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml rename to kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/default_joint_limits.yaml index 66e05b12..837816e2 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/joint_limits.yaml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/default_joint_limits.yaml @@ -4,34 +4,34 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.99 + max_velocity: 0.48 has_acceleration_limits: true - max_acceleration: 0.35 + max_acceleration: 0.86 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.99 + max_velocity: 0.48 has_acceleration_limits: true - max_acceleration: 0.17 + max_acceleration: 0.43 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.99 + max_velocity: 0.48 has_acceleration_limits: true - max_acceleration: 0.14 + max_acceleration: 0.34 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.99 + max_velocity: 0.76 has_acceleration_limits: true - max_acceleration: 0.35 + max_acceleration: 0.86 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.99 + max_velocity: 0.76 has_acceleration_limits: true - max_acceleration: 3.5 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.55 + max_velocity: 1.52 has_acceleration_limits: true - max_acceleration: 3.5 + max_acceleration: 8.6 right_finger_bottom_joint: has_velocity_limits: true max_velocity: 1000 diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro index bd5ef236..e193f220 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro @@ -39,7 +39,7 @@ - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/hard_joint_limits.yaml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/hard_joint_limits.yaml new file mode 100644 index 00000000..6bb653bd --- /dev/null +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/hard_joint_limits.yaml @@ -0,0 +1,39 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + $(arg prefix)joint_1: + has_velocity_limits: true + max_velocity: 1.52 + has_acceleration_limits: true + max_acceleration: 0.95 + $(arg prefix)joint_2: + has_velocity_limits: true + max_velocity: 1.52 + has_acceleration_limits: true + max_acceleration: 0.475 + $(arg prefix)joint_3: + has_velocity_limits: true + max_velocity: 1.52 + has_acceleration_limits: true + max_acceleration: 0.38 + $(arg prefix)joint_4: + has_velocity_limits: true + max_velocity: 1.52 + has_acceleration_limits: true + max_acceleration: 0.95 + $(arg prefix)joint_5: + has_velocity_limits: true + max_velocity: 1.52 + has_acceleration_limits: true + max_acceleration: 9.5 + $(arg prefix)joint_6: + has_velocity_limits: true + max_velocity: 3.04 + has_acceleration_limits: true + max_acceleration: 9.5 + right_finger_bottom_joint: + has_velocity_limits: true + max_velocity: 1000 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch index f26a35d8..2d148779 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/move_group.launch @@ -1,7 +1,6 @@ - @@ -9,6 +8,7 @@ + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml index b8cd5b6a..fdca963e 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -6,8 +6,7 @@ - - - - diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index 4ebcf357..52f720b8 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -8,6 +8,9 @@ + + + @@ -20,7 +23,8 @@ if="$(eval not arg('prefix'))"/> - + + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml index 7a09c04f..0e14e4c3 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/trajectory_execution.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml index 7a51e2c1..c0949cac 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/default_joint_limits.yaml @@ -4,31 +4,31 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 \ No newline at end of file + max_acceleration: 8.6 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml index f43b69a2..e432d1b5 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/hard_joint_limits.yaml @@ -4,31 +4,31 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 \ No newline at end of file + max_acceleration: 9.5 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml index 056e61f7..aab91ccb 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/default_joint_limits.yaml @@ -4,36 +4,36 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_7: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 \ No newline at end of file + max_acceleration: 8.6 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml index fdd996b5..74a3e6d9 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/hard_joint_limits.yaml @@ -4,36 +4,36 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_7: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 \ No newline at end of file + max_acceleration: 9.5 \ No newline at end of file diff --git a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml index bfe3d6c6..d923de34 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -6,8 +6,7 @@ - - - - diff --git a/kortex_move_it_config/gen3_move_it_config/package.xml b/kortex_move_it_config/gen3_move_it_config/package.xml index f8040e4a..2fca840d 100644 --- a/kortex_move_it_config/gen3_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_move_it_config/package.xml @@ -29,7 +29,6 @@ kortex_description kortex_description - industrial_trajectory_filters diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml index 83fdb6bc..2c4ec837 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/default_joint_limits.yaml @@ -4,34 +4,34 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml index 18bcded9..aa235da0 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/hard_joint_limits.yaml @@ -4,34 +4,34 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml index b172331f..169f2af7 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/default_joint_limits.yaml @@ -4,39 +4,39 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_7: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml index 7c291587..9365c62a 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/hard_joint_limits.yaml @@ -4,39 +4,39 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_7: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml index 99ec2a50..4c8c0d71 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -6,8 +6,7 @@ - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml index 50280983..fac89a05 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/package.xml @@ -25,7 +25,6 @@ joint_state_publisher robot_state_publisher xacro - industrial_trajectory_filters kortex_description diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml index 83fdb6bc..2c4ec837 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/default_joint_limits.yaml @@ -4,34 +4,34 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml index 18bcded9..aa235da0 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/hard_joint_limits.yaml @@ -4,34 +4,34 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml index b172331f..169f2af7 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/default_joint_limits.yaml @@ -4,39 +4,39 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 0.84 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)joint_7: has_velocity_limits: true - max_velocity: 0.86 + max_velocity: 0.41 has_acceleration_limits: true - max_acceleration: 0.3 + max_acceleration: 8.6 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml index 7c291587..9365c62a 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/hard_joint_limits.yaml @@ -4,39 +4,39 @@ joint_limits: $(arg prefix)joint_1: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_2: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_3: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_4: has_velocity_limits: true - max_velocity: 1.37 + max_velocity: 1.33 has_acceleration_limits: true - max_acceleration: 1.56 + max_acceleration: 4.94 $(arg prefix)joint_5: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_6: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)joint_7: has_velocity_limits: true - max_velocity: 1.15 + max_velocity: 1.16 has_acceleration_limits: true - max_acceleration: 3.0 + max_acceleration: 9.5 $(arg prefix)left_inner_finger_joint: has_velocity_limits: true max_velocity: 2 diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml index 5ef1a126..0ed973af 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/ompl_planning_pipeline.launch.xml @@ -6,8 +6,7 @@ - - - - diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml index 1e215044..e8a19dd6 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/package.xml @@ -25,7 +25,6 @@ joint_state_publisher robot_state_publisher xacro - industrial_trajectory_filters kortex_description diff --git a/kortex_move_it_config/readme.md b/kortex_move_it_config/readme.md index 610747ce..832f49ca 100644 --- a/kortex_move_it_config/readme.md +++ b/kortex_move_it_config/readme.md @@ -28,4 +28,4 @@ See the `kortex_description/grippers` folder for a list of supported Kinova Kort Upon launching the main launch file of a `move_it_config` package, `move_group.launch` (normally launched from the real arm driver's launch file and the simulation launch file), the [C++](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html) and [Python](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html) interfaces for MoveIt will be enabled. You will be able to use motion planning, configure the planning scene, send trajectories and send pose goals to the simulated robot from your own ROS nodes. -You can take a look at our [MoveIt! Python example](../kortex_examples/src/move_it/example_move_it_trajectories.py) for a concrete example. +You can take a look at our [MoveIt! Python example](../kortex_examples/python/move_it/example_move_it_trajectories.py) for a concrete example. diff --git a/readme.md b/readme.md index edd58121..e31fabb2 100644 --- a/readme.md +++ b/readme.md @@ -35,7 +35,6 @@ These are the instructions to run in a terminal to create the workspace, clone t rosdep install --from-paths src --ignore-src -y Then, to build and source the workspace: - catkin_make source devel/setup.bash