├── docs └── source │ ├── reference │ ├── limits.rst │ ├── sot_adapter.rst │ ├── sdk.rst │ ├── trajectory.rst │ ├── task.rst │ ├── cartesian.rst │ ├── postural.rst │ ├── problem.rst │ └── cartesian_ifc.rst │ ├── design.rst │ ├── features.png │ ├── tutorial_rviz.png │ ├── tasks │ ├── limitsros.rst │ ├── posturalros.rst │ ├── postural.rst │ ├── cartesian.rst │ └── limits.rst │ ├── plugins.rst │ ├── progr_py.rst │ └── porting.rst ├── msg ├── InteractionTaskInfo.msg ├── Impedance3.msg ├── CartesianImpedanceBase.msg ├── CartesianImpedanceTimed.msg ├── CartesianImpedance.msg ├── Impedance6.msg ├── JoystickStatus.msg ├── TaskInfo.msg └── CartesianTaskInfo.msg ├── srv ├── GetInteractionTaskInfo.srv ├── GetTaskList.srv ├── SetLambda.srv ├── SetWeight.srv ├── GetImpedance.srv ├── SetBaseLink.srv ├── SetControlMode.srv ├── SetTaskActive.srv ├── SetTransform.srv ├── ResetJoints.srv ├── SetJoystickActiveTask.srv ├── SetJoystickTaskMaxSpeed.srv ├── SetJoystickTaskBaseFrame.srv ├── SetLambda2.srv ├── ResetWorld.srv ├── SetImpedance.srv ├── SetContactFrame.srv ├── ToggleAxis.srv ├── SetSafetyLimits.srv ├── LoadController.srv ├── GetTaskInfo.srv └── GetCartesianTaskInfo.srv ├── .gitmodules ├── examples ├── mesh │ └── coman │ │ ├── Arm0.STL │ │ ├── Arm1.STL │ │ ├── Arm2.STL │ │ ├── Elbow.STL │ │ ├── Leg0.STL │ │ ├── Leg1.STL │ │ ├── Leg2.STL │ │ ├── Leg3.STL │ │ ├── Leg4.STL │ │ ├── Torso1.STL │ │ ├── Torso2.STL │ │ ├── Torso3.STL │ │ ├── Waist.STL │ │ ├── Forearm0.STL │ │ ├── Forearm1.STL │ │ ├── Forearm2.STL │ │ ├── ForearmOld.STL │ │ ├── RoundFoot.STL │ │ ├── XtionBase.STL │ │ ├── SoftHandOpen.STL │ │ ├── XtionCamBody.STL │ │ ├── Torso2.capsule │ │ ├── Elbow.capsule │ │ ├── Torso1.capsule │ │ ├── Forearm1.capsule │ │ ├── Torso3.capsule │ │ ├── Forearm0.capsule │ │ ├── Forearm2.capsule │ │ ├── XtionBase.capsule │ │ ├── XtionCamBody.capsule │ │ ├── ForearmOld.capsule │ │ ├── Waist.capsule │ │ ├── Leg0.capsule │ │ ├── Arm0.capsule │ │ ├── Leg2.capsule │ │ ├── Leg3.capsule │ │ ├── Leg4.capsule │ │ ├── Arm1.capsule │ │ ├── Arm2.capsule │ │ ├── Leg1.capsule │ │ ├── RoundFoot.capsule │ │ └── SoftHandOpen.capsule ├── plugin │ ├── CMakeLists.txt │ ├── min_effort │ │ ├── CMakeLists.txt │ │ └── src │ │ │ ├── OpenSotMinEffort.h │ │ │ ├── OpenSotMinEffort.cpp │ │ │ ├── MinEffort.h │ │ │ └── MinEffort.cpp │ ├── manipulability │ │ ├── CMakeLists.txt │ │ └── src │ │ │ ├── OpenSotManipulability.h │ │ │ ├── Manipulability.h │ │ │ ├── OpenSotManipulability.cpp │ │ │ └── Manipulability.cpp │ └── angular_mom │ │ ├── CMakeLists.txt │ │ └── src │ │ ├── OpenSotAngularMomentum.h │ │ ├── OpenSotAngularMomentum.cpp │ │ ├── AngularMomentumRos.h │ │ ├── AngularMomentumRos.cpp │ │ ├── AngularMomentum.h │ │ └── AngularMomentum.cpp ├── cpp │ └── CMakeLists.txt ├── launch │ ├── coman_acceleration_max_computer.launch │ └── coman.launch ├── configs │ ├── coman_min_effort.yaml │ ├── coman_stack_alternative.yaml │ └── coman_manipulability.yaml └── CMakeLists.txt ├── include └── cartesian_interface │ ├── external │ └── fmt │ │ └── posix.h │ ├── problem │ ├── Subtask.h │ ├── Gaze.h │ ├── Com.h │ ├── MinJointVel.h │ ├── Limits.h │ ├── Constraint.h │ ├── Postural.h │ └── Interaction.h │ ├── sdk │ ├── opensot │ │ ├── OpenSotUtils.h │ │ ├── OpenSotOmniWheels4X.h │ │ ├── OpenSotSubtask.h │ │ ├── OpenSotCom.h │ │ ├── OpenSotPostural.h │ │ ├── OpenSotVelocityLimits.h │ │ ├── OpenSotGaze.h │ │ ├── OpenSotCartesian.h │ │ ├── OpenSotConstraintFromTask.h │ │ ├── OpenSotInteraction.h │ │ ├── OpenSotJointLimits.h │ │ └── Plugin.h │ ├── problem │ │ ├── Com.h │ │ ├── Gaze.h │ │ ├── OmniWheels4X.h │ │ ├── Subtask.h │ │ ├── Constraint.h │ │ ├── Plugin.h │ │ └── Postural.h │ ├── ros │ │ ├── RosContext.h │ │ ├── server_api │ │ │ └── PosturalRos.h │ │ ├── client_api │ │ │ └── PosturalRos.h │ │ └── Plugin.h │ ├── SolverPlugin.h │ └── rt │ │ ├── LockfreeBufferImpl.h │ │ └── CInteractionRt.h │ ├── utils │ ├── RobotStatePublisher.h │ ├── AccMaxComputer.h │ ├── TaskFactory.h │ └── Manipulability.h │ ├── Context.h │ ├── Macro.h │ ├── Enum.h │ └── trajectory │ ├── Spline.h │ └── Trajectory.h ├── src ├── xbot2 │ ├── CMakeLists.txt │ └── cartesioplugin.h ├── ros │ ├── RosServerNode.cpp │ ├── RosContext.cpp │ ├── AccMaxComputerNode.cpp │ ├── CartesianAnalyzer.cpp │ ├── server_api │ │ └── PosturalRos.cpp │ └── client_api │ │ └── PosturalRos.cpp ├── problem │ └── impl │ │ ├── Com.cpp │ │ ├── Gaze.cpp │ │ ├── OmniWheels4X.cpp │ │ └── Constraint.cpp ├── plugin │ └── angular_mom │ │ ├── OpenSotAngularMomentum.h │ │ ├── AngularMomentumRos.h │ │ ├── OpenSotAngularMomentum.cpp │ │ ├── AngularMomentumRos.cpp │ │ ├── AngularMomentum.h │ │ └── AngularMomentum.cpp ├── utils │ ├── RobotStatePublisherNode.cpp │ ├── RobotStatePublisher.cpp │ └── DynamicLoading.h ├── Context.cpp └── opensot │ ├── task_adapters │ ├── OpenSotVelocityLimits.cpp │ ├── OpenSotOmniWheels4X.cpp │ ├── OpenSotSubtask.cpp │ ├── OpenSotPostural.cpp │ ├── OpenSotConstraintFromTask.cpp │ ├── OpenSotCom.cpp │ ├── OpenSotInteraction.cpp │ ├── OpenSotGaze.cpp │ └── OpenSotCartesian.cpp │ └── OpenSotImpl.h ├── bindings └── python │ ├── interactive_client.py │ ├── pyForceEstimation.cpp │ ├── pyci_all.py │ ├── pyImpedance.cpp │ ├── pyRosInit.cpp │ └── CMakeLists.txt ├── README.md ├── action ├── ReachPose.action └── ReachCartesianImpedance.action ├── launch ├── acceleration_max_computer.launch ├── cartesian_analyzer.launch ├── joystick.launch └── cartesio.launch ├── cmake ├── cartesian_interfaceConfig.cmake.in ├── GenerateDeb.cmake └── ExportCartesianInterface.cmake ├── .gitignore ├── tests ├── TestAlglib.cpp ├── configs │ └── centauro_test_stack.yaml └── CMakeLists.txt └── package.xml /docs/source/reference/limits.rst: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/source/reference/sot_adapter.rst: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /msg/InteractionTaskInfo.msg: -------------------------------------------------------------------------------- 1 | string state 2 | 3 | -------------------------------------------------------------------------------- /docs/source/design.rst: -------------------------------------------------------------------------------- 1 | Design notes 2 | ============ 3 | -------------------------------------------------------------------------------- /srv/GetInteractionTaskInfo.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string state 3 | 4 | -------------------------------------------------------------------------------- /msg/Impedance3.msg: -------------------------------------------------------------------------------- 1 | float32[9] stiffness 2 | float32[9] damping 3 | -------------------------------------------------------------------------------- /srv/GetTaskList.srv: -------------------------------------------------------------------------------- 1 | ---- 2 | string[] names 3 | string[] types 4 | -------------------------------------------------------------------------------- /docs/source/reference/sdk.rst: -------------------------------------------------------------------------------- 1 | SDK API Reference 2 | ================= 3 | -------------------------------------------------------------------------------- /srv/SetLambda.srv: -------------------------------------------------------------------------------- 1 | float32 lambda 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/SetWeight.srv: -------------------------------------------------------------------------------- 1 | float32[] weight 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/GetImpedance.srv: -------------------------------------------------------------------------------- 1 | --- 2 | cartesian_interface/CartesianImpedance impedance 3 | 4 | -------------------------------------------------------------------------------- /srv/SetBaseLink.srv: -------------------------------------------------------------------------------- 1 | string base_link 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/SetControlMode.srv: -------------------------------------------------------------------------------- 1 | string ctrl_mode 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/SetTaskActive.srv: -------------------------------------------------------------------------------- 1 | bool activation_state 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/SetTransform.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/ResetJoints.srv: -------------------------------------------------------------------------------- 1 | string[] joint_names 2 | float32[] joint_values 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /srv/SetJoystickActiveTask.srv: -------------------------------------------------------------------------------- 1 | string active_task 2 | ---- 3 | bool success 4 | string error_message 5 | -------------------------------------------------------------------------------- /srv/SetJoystickTaskMaxSpeed.srv: -------------------------------------------------------------------------------- 1 | float32 max_linear_speed 2 | float32 max_angular_speed 3 | ---- 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "external/fmt"] 2 | path = external/fmt 3 | url = git@github.com:fmtlib/fmt.git 4 | -------------------------------------------------------------------------------- /msg/CartesianImpedanceBase.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 stiffness 2 | geometry_msgs/Vector3 damping_ratio 3 | -------------------------------------------------------------------------------- /srv/SetJoystickTaskBaseFrame.srv: -------------------------------------------------------------------------------- 1 | string base_frame 2 | ---- 3 | bool success 4 | string error_message 5 | -------------------------------------------------------------------------------- /srv/SetLambda2.srv: -------------------------------------------------------------------------------- 1 | float32 lambda2 2 | bool auto_lambda2 3 | --- 4 | bool success 5 | string message 6 | -------------------------------------------------------------------------------- /srv/ResetWorld.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose new_world 2 | string from_link 3 | --- 4 | bool success 5 | string message -------------------------------------------------------------------------------- /docs/source/features.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/docs/source/features.png -------------------------------------------------------------------------------- /srv/SetImpedance.srv: -------------------------------------------------------------------------------- 1 | cartesian_interface/CartesianImpedance impedance 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /docs/source/reference/trajectory.rst: -------------------------------------------------------------------------------- 1 | Trajectory Interpolation API Reference 2 | ====================================== 3 | -------------------------------------------------------------------------------- /docs/source/tutorial_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/docs/source/tutorial_rviz.png -------------------------------------------------------------------------------- /examples/mesh/coman/Arm0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Arm0.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Arm1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Arm1.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Arm2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Arm2.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Elbow.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Elbow.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Leg0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Leg0.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Leg1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Leg1.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Leg2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Leg2.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Leg3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Leg3.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Leg4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Leg4.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Torso1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Torso1.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Torso2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Torso2.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Torso3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Torso3.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Waist.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Waist.STL -------------------------------------------------------------------------------- /include/cartesian_interface/external/fmt/posix.h: -------------------------------------------------------------------------------- 1 | #include "os.h" 2 | #warning "fmt/posix.h is deprecated; use fmt/os.h instead" -------------------------------------------------------------------------------- /examples/mesh/coman/Forearm0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Forearm0.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Forearm1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Forearm1.STL -------------------------------------------------------------------------------- /examples/mesh/coman/Forearm2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/Forearm2.STL -------------------------------------------------------------------------------- /msg/CartesianImpedanceTimed.msg: -------------------------------------------------------------------------------- 1 | cartesian_interface/CartesianImpedance impedance 2 | float32 time 3 | -------------------------------------------------------------------------------- /srv/SetContactFrame.srv: -------------------------------------------------------------------------------- 1 | string link_name 2 | geometry_msgs/Quaternion orientation 3 | --- 4 | bool success 5 | string message 6 | -------------------------------------------------------------------------------- /examples/mesh/coman/ForearmOld.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/ForearmOld.STL -------------------------------------------------------------------------------- /examples/mesh/coman/RoundFoot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/RoundFoot.STL -------------------------------------------------------------------------------- /examples/mesh/coman/XtionBase.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/XtionBase.STL -------------------------------------------------------------------------------- /msg/CartesianImpedance.msg: -------------------------------------------------------------------------------- 1 | cartesian_interface/CartesianImpedanceBase linear 2 | cartesian_interface/CartesianImpedanceBase angular 3 | -------------------------------------------------------------------------------- /msg/Impedance6.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | cartesian_interface/Impedance3 linear 4 | cartesian_interface/Impedance3 angular 5 | -------------------------------------------------------------------------------- /srv/ToggleAxis.srv: -------------------------------------------------------------------------------- 1 | # Select enable axis (tx, ty, tz, rx, ry, rz) 2 | bool[6] axis_mask 3 | --- 4 | string message 5 | bool success 6 | -------------------------------------------------------------------------------- /examples/mesh/coman/SoftHandOpen.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/SoftHandOpen.STL -------------------------------------------------------------------------------- /examples/mesh/coman/XtionCamBody.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ADVRHumanoids/CartesianInterface/HEAD/examples/mesh/coman/XtionCamBody.STL -------------------------------------------------------------------------------- /docs/source/tasks/limitsros.rst: -------------------------------------------------------------------------------- 1 | Joint limits ROS API 2 | ==================== 3 | 4 | 5 | Velocity limits ROS API 6 | ======================= 7 | -------------------------------------------------------------------------------- /docs/source/reference/task.rst: -------------------------------------------------------------------------------- 1 | Generic Task API 2 | ================ 3 | 4 | .. doxygenclass:: XBot::Cartesian::TaskDescription 5 | :members: 6 | -------------------------------------------------------------------------------- /examples/plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_subdirectory(angular_mom/) 3 | 4 | add_subdirectory(min_effort/) 5 | 6 | add_subdirectory(manipulability/) 7 | -------------------------------------------------------------------------------- /docs/source/reference/cartesian.rst: -------------------------------------------------------------------------------- 1 | Cartesian Task API 2 | ================== 3 | 4 | .. doxygenclass:: XBot::Cartesian::CartesianTask 5 | :members: 6 | -------------------------------------------------------------------------------- /docs/source/reference/postural.rst: -------------------------------------------------------------------------------- 1 | Postural Task API 2 | ================= 3 | 4 | .. doxygenstruct:: XBot::Cartesian::PosturalTask 5 | :members: 6 | -------------------------------------------------------------------------------- /srv/SetSafetyLimits.srv: -------------------------------------------------------------------------------- 1 | float32 max_vel_lin 2 | float32 max_vel_ang 3 | float32 max_acc_lin 4 | float32 max_acc_ang 5 | --- 6 | bool success 7 | string message 8 | -------------------------------------------------------------------------------- /msg/JoystickStatus.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | string active_task 4 | 5 | float32 max_linear_speed 6 | float32 max_angular_speed 7 | 8 | uint8[6] twist_mask 9 | -------------------------------------------------------------------------------- /srv/LoadController.srv: -------------------------------------------------------------------------------- 1 | string controller_name 2 | string problem_description_name 3 | string problem_description_string 4 | bool force_reload 5 | --- 6 | string message 7 | bool success 8 | -------------------------------------------------------------------------------- /msg/TaskInfo.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string[] type 3 | uint32 size 4 | uint32[] indices 5 | float32[] weight 6 | float32 lambda 7 | float32 lambda2 8 | string activation_state 9 | string[] disabled_joints 10 | string lib_name 11 | -------------------------------------------------------------------------------- /msg/CartesianTaskInfo.msg: -------------------------------------------------------------------------------- 1 | string base_link 2 | string distal_link 3 | string control_mode 4 | string state 5 | bool use_local_subtasks 6 | float64 max_vel_lin 7 | float64 max_vel_ang 8 | float64 max_acc_lin 9 | float64 max_acc_ang 10 | -------------------------------------------------------------------------------- /srv/GetTaskInfo.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string name 3 | string[] type 4 | float32 lambda 5 | float32 lambda2 6 | string activation_state 7 | float32[] weight 8 | uint32[] indices 9 | uint32 size 10 | string[] disabled_joints 11 | string lib_name 12 | -------------------------------------------------------------------------------- /src/xbot2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(xbot2 REQUIRED) 2 | 3 | add_xbot2_plugin(cartesio_plugin cartesioplugin.cpp) 4 | 5 | target_link_libraries(cartesio_plugin PUBLIC CartesianInterface) 6 | 7 | install(TARGETS cartesio_plugin 8 | DESTINATION lib) 9 | -------------------------------------------------------------------------------- /srv/GetCartesianTaskInfo.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string base_link 3 | string distal_link 4 | string control_mode 5 | string state 6 | bool use_local_subtasks 7 | float64 max_vel_lin 8 | float64 max_vel_ang 9 | float64 max_acc_lin 10 | float64 max_acc_ang 11 | -------------------------------------------------------------------------------- /bindings/python/interactive_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/ipython -i 2 | 3 | from cartesian_interface.pyci_all import * 4 | import numpy as np 5 | 6 | ci = pyci.CartesianInterfaceRos() 7 | tasks = ci.getTaskList() 8 | 9 | for t in tasks: 10 | exec(t + '= ci.getTask(\'{}\')'.format(t)) 11 | -------------------------------------------------------------------------------- /src/ros/RosServerNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | int main(int argc, char ** argv) 6 | { 7 | ros::init(argc, argv, "ros_server_node"); 8 | 9 | RosExecutor exec; 10 | 11 | exec.spin(); 12 | 13 | } 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Cartesian Interface 2 | Package for generic cartesian control of floating base robots. 3 | It includes a ROS-based front end, as well as a programmatic API 4 | that can be used inside a real-time loop. 5 | 6 | Have a look to the [**documentation!**](https://advrhumanoids.github.io/CartesianInterface/) 7 | -------------------------------------------------------------------------------- /src/problem/impl/Com.cpp: -------------------------------------------------------------------------------- 1 | #include "problem/Com.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | ComTaskImpl::ComTaskImpl(Context::ConstPtr context): 6 | CartesianTaskImpl(context, "com", "com", "world") 7 | { 8 | 9 | } 10 | 11 | ComTaskImpl::ComTaskImpl(YAML::Node node, 12 | Context::ConstPtr context): 13 | CartesianTaskImpl(node, context) 14 | { 15 | 16 | } 17 | 18 | -------------------------------------------------------------------------------- /action/ReachPose.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | geometry_msgs/Pose[] frames 3 | float32[] time 4 | bool incremental 5 | --- 6 | # Define the result 7 | geometry_msgs/Pose final_frame 8 | float32 position_error_norm 9 | float32 orientation_error_angle 10 | --- 11 | # Define a feedback message 12 | geometry_msgs/PoseStamped current_reference 13 | geometry_msgs/PoseStamped current_pose 14 | int32 current_segment_id 15 | -------------------------------------------------------------------------------- /examples/cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | add_executable(cartesio_solver cartesio_solver.cpp) 4 | target_compile_definitions(cartesio_solver PRIVATE 5 | -DURDF_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../urdf/coman.urdf" 6 | -DSRDF_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../srdf/coman.srdf" 7 | -DIK_PB_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../configs/coman_stack.yaml") 8 | target_link_libraries(cartesio_solver CartesianInterface) 9 | -------------------------------------------------------------------------------- /launch/acceleration_max_computer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /cmake/cartesian_interfaceConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | set_and_check(@PROJECT_NAME@_TARGETS 4 | "${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 5 | 6 | include(CMakeFindDependencyMacro) 7 | find_dependency(xbot2_interface) 8 | #find_dependency(estimation_utils) 9 | find_dependency(OpenSoT) 10 | find_dependency(matlogger2) 11 | 12 | include(${@PROJECT_NAME@_TARGETS}) 13 | 14 | set(@PROJECT_NAME@_LIBRARIES cartesian_interface::CartesianInterface) 15 | -------------------------------------------------------------------------------- /examples/mesh/coman/Torso2.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | *.kdev4 35 | docs/xml/* 36 | *.user 37 | .vscode/* 38 | -------------------------------------------------------------------------------- /examples/mesh/coman/Elbow.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/Torso1.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/Forearm1.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/Torso3.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/Forearm0.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/Forearm2.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/XtionBase.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/XtionCamBody.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/ForearmOld.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/mesh/coman/Waist.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /examples/plugin/min_effort/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(CartesioMinimumEffortAddon SHARED 2 | src/MinEffort.cpp 3 | src/OpenSotMinEffort.cpp 4 | ) 5 | 6 | target_link_libraries(CartesioMinimumEffortAddon PRIVATE 7 | ${OpenSoT_LIBRARIES} 8 | CartesianInterface) 9 | 10 | install(TARGETS CartesioMinimumEffortAddon 11 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 12 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 13 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 14 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Subtask.h: -------------------------------------------------------------------------------- 1 | #ifndef SUBTASK_H 2 | #define SUBTASK_H 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | class Subtask : public virtual TaskDescription 9 | { 10 | 11 | public: 12 | 13 | CARTESIO_DECLARE_SMART_PTR(Subtask) 14 | 15 | virtual TaskDescription::Ptr getTask() = 0; 16 | 17 | virtual const std::vector& getSubtaskIndices() const = 0; 18 | 19 | }; 20 | 21 | } } 22 | 23 | #endif // SUBTASK_H 24 | -------------------------------------------------------------------------------- /docs/source/tasks/posturalros.rst: -------------------------------------------------------------------------------- 1 | Postural task ROS API 2 | ===================== 3 | 4 | 5 | Contents: 6 | 7 | .. contents:: :local: 8 | 9 | 10 | Topics 11 | ------ 12 | 13 | reference (input) 14 | ^^^^^^^^^^^^^^^^^ 15 | Input topic for user-provided joint references. Type: ``sensors_msgs/JointState``. 16 | 17 | current_reference (output) 18 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 19 | Output topic containing the most recent joint reference tracked by the controller. 20 | Type: ``sensors_msgs/JointState``. 21 | 22 | 23 | -------------------------------------------------------------------------------- /examples/plugin/manipulability/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(CartesioManipulabilityAddon SHARED 2 | src/Manipulability.cpp 3 | src/OpenSotManipulability.cpp 4 | ) 5 | 6 | target_link_libraries(CartesioManipulabilityAddon PRIVATE 7 | ${OpenSoT_LIBRARIES} 8 | CartesianInterface) 9 | 10 | install(TARGETS CartesioManipulabilityAddon 11 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 12 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 13 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 14 | -------------------------------------------------------------------------------- /src/problem/impl/Gaze.cpp: -------------------------------------------------------------------------------- 1 | #include "problem/Gaze.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | GazeTaskImpl::GazeTaskImpl(Context::ConstPtr context, 6 | std::string name, 7 | std::string distal_link, 8 | std::string base_link): 9 | CartesianTaskImpl(context, name, distal_link, base_link) 10 | { 11 | 12 | } 13 | 14 | GazeTaskImpl::GazeTaskImpl(YAML::Node node, 15 | Context::ConstPtr context): 16 | CartesianTaskImpl(node, context) 17 | { 18 | 19 | } 20 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Gaze.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_GAZE_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_GAZE_H__ 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief Description of a Gaze task 10 | */ 11 | struct GazeTask : virtual CartesianTask { 12 | 13 | typedef std::shared_ptr Ptr; 14 | typedef std::shared_ptr ConstPtr; 15 | }; 16 | 17 | 18 | } } 19 | 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Com.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_COM_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_COM_H__ 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief Description of a center of mass task 10 | */ 11 | struct ComTask : virtual CartesianTask { 12 | 13 | typedef std::shared_ptr Ptr; 14 | typedef std::shared_ptr ConstPtr; 15 | }; 16 | 17 | } } 18 | 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /docs/source/reference/problem.rst: -------------------------------------------------------------------------------- 1 | .. _cppref: 2 | 3 | Problem Description API Reference 4 | ================================= 5 | 6 | The header files in ```` contain the API definition for all natively supported 7 | tasks. These are abstract classes which are inherited by: 8 | - implementation classes, which define the runtime behavior of tasks (e.g. reference management, 9 | safety features, logging, ...) 10 | - client APIs, such as ROS client classes 11 | 12 | 13 | .. toctree:: 14 | :maxdepth: 1 15 | 16 | task 17 | cartesian 18 | postural 19 | -------------------------------------------------------------------------------- /action/ReachCartesianImpedance.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | 3 | cartesian_interface/CartesianImpedanceTimed[] target 4 | 5 | --- 6 | 7 | # Result 8 | 9 | cartesian_interface/CartesianImpedance impedance_final 10 | float32 error_mean 11 | float32 error_sigma 12 | 13 | --- 14 | 15 | # Feedback message 16 | 17 | cartesian_interface/CartesianImpedance impedance_reference 18 | cartesian_interface/CartesianImpedance impedance_actual 19 | float32 progress 20 | float32 time_to_finish 21 | 22 | -------------------------------------------------------------------------------- /launch/cartesian_analyzer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /examples/launch/coman_acceleration_max_computer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /docs/source/reference/cartesian_ifc.rst: -------------------------------------------------------------------------------- 1 | .. _cartesian_ifc: 2 | 3 | CartesianInterface API Reference 4 | ================================ 5 | 6 | The header files ``CartesianInterface.h`` and ``CartesianInterfaceImpl.h`` 7 | contain the API definition for the main CartesIO class, which users can load 8 | via the provided factory method ``CartesianInterfaceImpl::MakeInstance()``. 9 | After loading, the recommended way to interact with the defined tasks 10 | and constraint is to use their own specific APIs, as 11 | obtained via the getTask() method. 12 | 13 | 14 | .. doxygenclass:: XBot::Cartesian::CartesianInterface 15 | :members: 16 | 17 | .. doxygenclass:: XBot::Cartesian::CartesianInterfaceImpl 18 | :members: 19 | 20 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotUtils.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTUTILS_H 2 | #define OPENSOTUTILS_H 3 | 4 | #include 5 | 6 | // if version is exported, we are using a recent enough opensot -> std::make_shared 7 | // otherwise, we are using an old one -> boost::make_shared 8 | #ifdef OPENSOT_VERSION_MAJOR 9 | 10 | #include 11 | namespace XBot { namespace Cartesian { namespace SotUtils { 12 | using std::make_shared; 13 | using std::shared_ptr; 14 | } } } 15 | 16 | #else 17 | 18 | #include 19 | namespace XBot { namespace Cartesian { namespace SotUtils { 20 | using boost::make_shared; 21 | using boost::shared_ptr; 22 | } } } 23 | 24 | #endif 25 | 26 | #endif // OPENSOTUTILS_H 27 | -------------------------------------------------------------------------------- /docs/source/tasks/postural.rst: -------------------------------------------------------------------------------- 1 | Postural task 2 | ============= 3 | The postural task has two peculiarities w.r.t. a generic *Task*. First, subtasks of a Postural task 4 | are not specified with the ``indices`` field, but only through the ``enabled_joints`` or 5 | ``disabled_joint`` fields. Second, the ``weight`` field can be expressed as a **map** between 6 | **joint_name** and **weight value**- 7 | 8 | Example 9 | ------- 10 | 11 | .. code-block:: yaml 12 | 13 | MyPostural: 14 | type: Postural 15 | lambda: 0.01 16 | weight: 17 | joint1: 10 18 | joint2: 10 19 | # all missing joints will be given weight equal 1.0 20 | disabled_joints: 21 | - joint3 22 | - joint4 23 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/Com.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_COM_IMPL_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_COM_IMPL_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | /** 10 | * @brief Description of a center of mass task 11 | */ 12 | struct ComTaskImpl : public CartesianTaskImpl, 13 | public virtual ComTask 14 | { 15 | 16 | CARTESIO_DECLARE_SMART_PTR(ComTaskImpl) 17 | 18 | ComTaskImpl(Context::ConstPtr context); 19 | 20 | ComTaskImpl(YAML::Node node, Context::ConstPtr context); 21 | }; 22 | 23 | } } 24 | 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/MinJointVel.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_MINJOINTVEL_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_MINJOINTVEL_H__ 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | struct MinJointVelTask : TaskDescription { 9 | typedef std::shared_ptr Ptr; 10 | typedef std::shared_ptr ConstPtr; 11 | 12 | MinJointVelTask(int ndof); 13 | 14 | double eps; 15 | 16 | static TaskDescription::Ptr yaml_parse_minjointvel(YAML::Node node, ModelInterface::ConstPtr model); 17 | }; 18 | 19 | MinJointVelTask::Ptr MakeMinJointVel(int ndof); 20 | 21 | MinJointVelTask::Ptr GetAsMinJointVel(TaskDescription::Ptr task); 22 | 23 | } 24 | } 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /examples/configs/coman_min_effort.yaml: -------------------------------------------------------------------------------- 1 | solver_options: 2 | regularization: 1e-6 3 | back_end: "qpoases" 4 | stack: 5 | - ["Base"] 6 | - ["MinEffort"] 7 | 8 | constraints: ["JointLimits", "VelocityLimits"] 9 | 10 | Base: 11 | type: "Cartesian" 12 | name: "base" 13 | distal_link: "base_link" 14 | lambda: 0.1 15 | 16 | MinEffort: 17 | type: MinimumEffort 18 | lib_name: libCartesioMinimumEffortAddon.so 19 | step_size: 1e-2 20 | lambda: 0.02 21 | weight: 22 | reference@v0: 0.0 23 | reference@v1: 0.0 24 | reference@v2: 0.0 25 | reference@v3: 0.0 26 | reference@v4: 0.0 27 | reference@v5: 0.0 28 | 29 | JointLimits: 30 | type: "JointLimits" 31 | 32 | 33 | VelocityLimits: 34 | type: "VelocityLimits" 35 | -------------------------------------------------------------------------------- /examples/mesh/coman/Leg0.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Arm0.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Leg2.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Leg3.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Leg4.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Arm1.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Arm2.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/Leg1.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/RoundFoot.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/mesh/coman/SoftHandOpen.capsule: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #AngularMomentumAddon (Velocity) 2 | add_library(CartesioAngularMomentumAddon SHARED 3 | src/AngularMomentum.cpp 4 | src/OpenSotAngularMomentum.cpp 5 | src/AngularMomentumRos.cpp) 6 | 7 | target_link_libraries(CartesioAngularMomentumAddon PRIVATE 8 | ${OpenSoT_LIBRARIES} 9 | CartesianInterface) 10 | 11 | install(TARGETS CartesioAngularMomentumAddon 12 | EXPORT ${PROJECT_NAME}Targets 13 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 14 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 15 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 16 | 17 | # Mark cpp header files for installation 18 | install(FILES "src/AngularMomentum.h" "src/AngularMomentumRos.h" 19 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 20 | ) 21 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/ros/RosContext.h: -------------------------------------------------------------------------------- 1 | #ifndef ROSCONTEXT_H 2 | #define ROSCONTEXT_H 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | class RosContext 10 | { 11 | 12 | public: 13 | 14 | CARTESIO_DECLARE_SMART_PTR(RosContext) 15 | 16 | RosContext(ros::NodeHandle nh, 17 | std::string tf_prefix, 18 | Context::ConstPtr ci_context); 19 | 20 | ros::NodeHandle& nh(); 21 | const std::string& tf_prefix() const; 22 | const std::string& tf_prefix_slash() const; 23 | Context::ConstPtr ci_context() const; 24 | 25 | private: 26 | 27 | ros::NodeHandle _nh; 28 | std::string _tf_prefix; 29 | std::string _tf_prefix_slash; 30 | Context::ConstPtr _ci_ctx; 31 | 32 | }; 33 | 34 | } } 35 | 36 | #endif // ROSCONTEXT_H 37 | -------------------------------------------------------------------------------- /src/ros/RosContext.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/RosContext.h" 2 | 3 | #include 4 | 5 | using namespace XBot::Cartesian; 6 | 7 | 8 | RosContext::RosContext(ros::NodeHandle nh, 9 | std::string tf_prefix, 10 | Context::ConstPtr ci_context): 11 | _nh(nh), 12 | _tf_prefix(tf_prefix), 13 | _tf_prefix_slash(tf_prefix == "" ? "" : (tf_prefix + "/")), 14 | _ci_ctx(ci_context) 15 | { 16 | } 17 | 18 | 19 | ros::NodeHandle & RosContext::nh() 20 | { 21 | return _nh; 22 | } 23 | 24 | const std::string & RosContext::tf_prefix() const 25 | { 26 | return _tf_prefix; 27 | } 28 | 29 | const std::string & RosContext::tf_prefix_slash() const 30 | { 31 | return _tf_prefix_slash; 32 | } 33 | 34 | Context::ConstPtr RosContext::ci_context() const 35 | { 36 | return _ci_ctx; 37 | } 38 | 39 | -------------------------------------------------------------------------------- /bindings/python/pyForceEstimation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace py = pybind11; 6 | using namespace XBot::Cartesian::Utils; 7 | 8 | PYBIND11_MODULE(pyest, m) { 9 | 10 | py::class_(m, "ForceEstimation") 11 | .def(py::init()) 12 | .def("addLink", &ForceEstimation::add_link) 13 | .def("update", &ForceEstimation::update) 14 | .def("setIgnoredJoint", &ForceEstimation::setIgnoredJoint) 15 | ; 16 | 17 | py::class_(m, "ForceEstimationMomentumBased") 18 | .def(py::init(), py::arg("model"), py::arg("rate"), py::arg("svd_th"), py::arg("obs_bw")) 19 | ; 20 | 21 | } 22 | 23 | -------------------------------------------------------------------------------- /include/cartesian_interface/utils/RobotStatePublisher.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTSTATEPUBLISHER_H 2 | #define ROBOTSTATEPUBLISHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace XBot { namespace Cartesian { namespace Utils { 9 | 10 | class RobotStatePublisher 11 | { 12 | 13 | public: 14 | 15 | RobotStatePublisher(ModelInterface::ConstPtr model); 16 | 17 | void publishTransforms(const ros::Time& time, 18 | const std::string& tf_prefix); 19 | 20 | 21 | private: 22 | 23 | ModelInterface::ConstPtr _model; 24 | tf2_ros::TransformBroadcaster _tf_broadcaster; 25 | tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster; 26 | std::vector _tf_vector; 27 | 28 | }; 29 | 30 | } } } 31 | 32 | #endif // ROBOTSTATEPUBLISHER_H 33 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/Gaze.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_GAZE_IMPL_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_GAZE_IMPL_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | /** 10 | * @brief Description of a center of mass task 11 | */ 12 | struct GazeTaskImpl : public CartesianTaskImpl, 13 | public virtual GazeTask 14 | { 15 | 16 | CARTESIO_DECLARE_SMART_PTR(GazeTaskImpl) 17 | 18 | 19 | GazeTaskImpl(Context::ConstPtr context, 20 | std::string name, 21 | std::string distal_link, 22 | std::string base_link = "world"); 23 | 24 | GazeTaskImpl(YAML::Node node, Context::ConstPtr context); 25 | }; 26 | 27 | } } 28 | 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(plugin) 2 | add_subdirectory(cpp) 3 | 4 | install(DIRECTORY configs/ 5 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/configs 6 | FILES_MATCHING PATTERN "*.yaml" 7 | ) 8 | 9 | install(DIRECTORY launch/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/launch 11 | FILES_MATCHING PATTERN "*.launch" 12 | ) 13 | 14 | install(DIRECTORY mesh/ 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/mesh 16 | ) 17 | 18 | install(DIRECTORY srdf/ 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/srdf 20 | ) 21 | 22 | install(DIRECTORY urdf/ 23 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/urdf 24 | ) 25 | 26 | install(DIRECTORY rviz/ 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/rviz 28 | ) 29 | 30 | install(DIRECTORY python/ 31 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples/python 32 | ) 33 | 34 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/ros/server_api/PosturalRos.h: -------------------------------------------------------------------------------- 1 | #ifndef POSTURALROS_SERVERAPI_H 2 | #define POSTURALROS_SERVERAPI_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace XBot { namespace Cartesian { 10 | 11 | namespace ServerApi 12 | { 13 | class PosturalRos; 14 | } 15 | 16 | class ServerApi::PosturalRos : public ServerApi::TaskRos 17 | { 18 | 19 | public: 20 | 21 | PosturalRos(PosturalTask::Ptr task, 22 | RosContext::Ptr context); 23 | 24 | virtual void run(ros::Time time) override; 25 | 26 | protected: 27 | 28 | private: 29 | 30 | void on_ref_recv(sensor_msgs::JointStateConstPtr msg); 31 | 32 | PosturalTask::Ptr _postural; 33 | 34 | ros::Publisher _current_ref_pub; 35 | ros::Subscriber _ref_sub; 36 | 37 | Eigen::VectorXd _posture_ref; 38 | 39 | 40 | 41 | 42 | 43 | }; 44 | 45 | } } 46 | 47 | #endif // POSTURALROS_H 48 | -------------------------------------------------------------------------------- /bindings/python/pyci_all.py: -------------------------------------------------------------------------------- 1 | from cartesian_interface import pyci 2 | from cartesian_interface import roscpp_utils as roscpp 3 | from cartesian_interface import pyest 4 | from xbot2_interface.pyaffine3 import Affine3 5 | from cartesian_interface.impedance import Impedance 6 | 7 | 8 | def get_xbot_config(is_floating_base=True, prefix='', model_type='RBDL', urdf=None, srdf=None, framework='ROS'): 9 | import rospy 10 | from xbot_interface import config_options as co 11 | 12 | if urdf is None: 13 | urdf = rospy.get_param(prefix + 'robot_description') 14 | 15 | if srdf is None: 16 | srdf = rospy.get_param(prefix + 'robot_description_semantic') 17 | 18 | cfg = co.ConfigOptions() 19 | cfg.set_urdf(urdf) 20 | cfg.set_srdf(srdf) 21 | cfg.generate_jidmap() 22 | cfg.set_string_parameter('model_type', model_type) 23 | cfg.set_bool_parameter('is_model_floating_base', is_floating_base) 24 | 25 | cfg.set_string_parameter('framework', framework) 26 | 27 | return cfg 28 | 29 | -------------------------------------------------------------------------------- /examples/plugin/min_effort/src/OpenSotMinEffort.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTMinimumEffort_H 2 | #define OPENSOTMinimumEffort_H 3 | 4 | #include "MinEffort.h" 5 | #include 6 | 7 | #include 8 | 9 | using MinimumEffortSoT = OpenSoT::tasks::velocity::MinimumEffort; 10 | 11 | namespace XBot { namespace Cartesian { 12 | 13 | 14 | class OpenSotMinimumEffort : public OpenSotTaskAdapter 15 | { 16 | 17 | public: 18 | 19 | OpenSotMinimumEffort(TaskDescription::Ptr task, 20 | Context::ConstPtr context); 21 | 22 | virtual TaskPtr constructTask() override; 23 | 24 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 25 | 26 | virtual void update(double time, double period) override; 27 | 28 | virtual ~OpenSotMinimumEffort() override = default; 29 | 30 | private: 31 | 32 | MinimumEffortSoT::Ptr _sot_task; 33 | MinimumEffort::Ptr _ci_task; 34 | 35 | 36 | }; 37 | 38 | } } 39 | 40 | #endif // OPENSOTMinimumEffort_H 41 | -------------------------------------------------------------------------------- /tests/TestAlglib.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main() 7 | { 8 | auto logger = XBot::MatLogger::getLogger("/tmp/alglib_test"); 9 | 10 | Eigen::VectorXd x(5), y(5); 11 | x << 1, 2, 3, 4, 5; 12 | y.setRandom(5); 13 | 14 | logger->log("xk", x); 15 | logger->log("yk", y); 16 | 17 | alglib::real_1d_array x_alg, y_alg; 18 | x_alg.attach_to_ptr(x.size(), x.data()); 19 | y_alg.attach_to_ptr(y.size(), y.data()); 20 | 21 | alglib::spline1dinterpolant spline; 22 | alglib::spline1dbuildcubic(x_alg, y_alg, 5, 1, 0, 1, 0, spline); 23 | 24 | 25 | for(double x_val = 0; x_val < 6; x_val += 0.001) 26 | { 27 | double y_val = alglib::spline1dcalc(spline, x_val); 28 | logger->add("x", x_val); 29 | logger->add("y", y_val); 30 | } 31 | 32 | 33 | 34 | 35 | logger->flush(); 36 | 37 | return 0; 38 | 39 | } -------------------------------------------------------------------------------- /src/plugin/angular_mom/OpenSotAngularMomentum.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTANGULARMOMENTUM_H 2 | #define OPENSOTANGULARMOMENTUM_H 3 | 4 | #include "AngularMomentum.h" 5 | #include 6 | 7 | #include 8 | 9 | using AngularMomentumSoT = OpenSoT::tasks::velocity::AngularMomentum; 10 | 11 | namespace XBot { namespace Cartesian { 12 | 13 | 14 | class OpenSotAngularMomentum : public OpenSotTaskAdapter 15 | { 16 | 17 | public: 18 | 19 | OpenSotAngularMomentum(TaskDescription::Ptr task, 20 | ModelInterface::ConstPtr model); 21 | 22 | virtual TaskPtr constructTask() override; 23 | 24 | virtual bool initialize() override; 25 | 26 | virtual void update(double time, double period) override; 27 | 28 | virtual ~OpenSotAngularMomentum() override = default; 29 | 30 | private: 31 | 32 | AngularMomentumSoT::Ptr _sot_angmom; 33 | AngularMomentum::Ptr _ci_angmom; 34 | 35 | 36 | }; 37 | 38 | } } 39 | 40 | #endif // OPENSOTANGULARMOMENTUM_H 41 | -------------------------------------------------------------------------------- /src/utils/RobotStatePublisherNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace XBot::Cartesian; 5 | 6 | int main(int argc, char ** argv) 7 | { 8 | ros::init(argc, argv, "robot_state_publisher"); 9 | ros::NodeHandle npr("~"); 10 | 11 | auto opt = Utils::LoadOptionsFromParamServer(); 12 | 13 | XBot::ModelInterface::Ptr model = XBot::ModelInterface::getModel(opt); 14 | 15 | Eigen::VectorXd qhome; 16 | model->getRobotState("home", qhome); 17 | model->setJointPosition(qhome); 18 | model->update(); 19 | 20 | Utils::RobotStatePublisher rspub(model); 21 | 22 | auto tf_prefix = npr.param("tf_prefix", ""); 23 | double rate = npr.param("rate", 100); 24 | 25 | auto on_timer_event = [&rspub, tf_prefix](const ros::TimerEvent& ev) 26 | { 27 | rspub.publishTransforms(ev.current_real, tf_prefix); 28 | }; 29 | 30 | auto timer = npr.createTimer(ros::Duration(1./rate), on_timer_event); 31 | 32 | ros::spin(); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /examples/launch/coman.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 26 | output="screen"/> 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/src/OpenSotAngularMomentum.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTANGULARMOMENTUM_H 2 | #define OPENSOTANGULARMOMENTUM_H 3 | 4 | #include "AngularMomentum.h" 5 | #include 6 | 7 | #include 8 | 9 | using AngularMomentumSoT = OpenSoT::tasks::velocity::AngularMomentum; 10 | 11 | namespace XBot { namespace Cartesian { 12 | 13 | 14 | class OpenSotAngularMomentum : public OpenSotTaskAdapter 15 | { 16 | 17 | public: 18 | 19 | OpenSotAngularMomentum(TaskDescription::Ptr task, 20 | Context::ConstPtr context); 21 | 22 | virtual TaskPtr constructTask() override; 23 | 24 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 25 | 26 | virtual void update(double time, double period) override; 27 | 28 | virtual ~OpenSotAngularMomentum() override = default; 29 | 30 | private: 31 | 32 | AngularMomentumSoT::Ptr _sot_angmom; 33 | AngularMomentum::Ptr _ci_angmom; 34 | 35 | 36 | }; 37 | 38 | } } 39 | 40 | #endif // OPENSOTANGULARMOMENTUM_H 41 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/OmniWheels4X.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_OMNIWHEELS4X_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_OMNIWHEELS4X_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | class OmniWheels4X : public virtual ConstraintDescription, public virtual TaskDescriptionImpl 10 | { 11 | 12 | public: 13 | 14 | CARTESIO_DECLARE_SMART_PTR(OmniWheels4X) 15 | 16 | OmniWheels4X(YAML::Node yaml, Context::ConstPtr context); 17 | 18 | double getl1() { return _l1; } 19 | double getl2() { return _l2; } 20 | double getr() { return _r; } 21 | const std::string& get_base_link() const { return _base_link; } 22 | const std::vector get_joint_wheels_name() const { return _joint_wheels_name; } 23 | 24 | private: 25 | 26 | double _l1, _l2, _r; 27 | std::vector _joint_wheels_name; 28 | std::string _base_link; 29 | }; 30 | 31 | 32 | 33 | } } 34 | 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /src/plugin/angular_mom/AngularMomentumRos.h: -------------------------------------------------------------------------------- 1 | #ifndef ANGULARMOMENTUMROS_H 2 | #define ANGULARMOMENTUMROS_H 3 | 4 | #include "AngularMomentum.h" 5 | #include 6 | #include 7 | 8 | namespace XBot { namespace Cartesian { 9 | 10 | namespace ServerApi 11 | { 12 | class AngularMomentumRos; 13 | } 14 | 15 | /** 16 | * @brief The ServerApi::AngularMomentumRos class implements a ROS 17 | * interface for the task. 18 | */ 19 | class ServerApi::AngularMomentumRos : public ServerApi::TaskRos 20 | { 21 | 22 | public: 23 | 24 | CARTESIO_DECLARE_SMART_PTR(AngularMomentumRos) 25 | 26 | AngularMomentumRos(TaskDescription::Ptr task, 27 | ModelInterface::ConstPtr model); 28 | 29 | void run(ros::Time time) override; 30 | 31 | 32 | private: 33 | 34 | void on_ref_recv(geometry_msgs::Vector3StampedConstPtr msg); 35 | 36 | ros::Subscriber _ref_sub; 37 | ros::Publisher _cur_ref_pub; 38 | 39 | AngularMomentum::Ptr _ci_angmom; 40 | 41 | 42 | }; 43 | 44 | } } 45 | 46 | #endif // ANGULARMOMENTUMROS_H 47 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotOmniWheels4X.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTOMNIWHEELS4X_H 2 | #define OPENSOTOMNIWHEELS4X_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | 11 | using OmniWheels4XSoT = OpenSoT::constraints::velocity::OmniWheels4X; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotOmniWheels4XAdapter : 16 | public OpenSotConstraintAdapter 17 | { 18 | 19 | public: 20 | 21 | OpenSotOmniWheels4XAdapter(ConstraintDescription::Ptr constr, 22 | Context::ConstPtr context); 23 | 24 | virtual ConstraintPtr constructConstraint() override; 25 | 26 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 27 | 28 | virtual ~OpenSotOmniWheels4XAdapter() override = default; 29 | 30 | protected: 31 | 32 | private: 33 | 34 | OmniWheels4XSoT::Ptr _opensot_omniwheels4x; 35 | OmniWheels4X::Ptr _ci_omniwheels4x; 36 | }; 37 | 38 | 39 | 40 | 41 | } } 42 | 43 | #endif // OPENSOTOMNIWHEELS4X_H 44 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotSubtask.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTSUBTASK_H 2 | #define OPENSOTSUBTASK_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | 11 | using SubtaskSoT = OpenSoT::SubTask; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotSubtaskAdapter : 16 | public OpenSotTaskAdapter 17 | { 18 | 19 | public: 20 | 21 | OpenSotSubtaskAdapter(TaskDescription::Ptr task, 22 | Context::ConstPtr context); 23 | 24 | OpenSoT::OptvarHelper::VariableVector getRequiredVariables() const override; 25 | 26 | virtual TaskPtr constructTask() override; 27 | 28 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 29 | 30 | virtual void update(double time, double period) override; 31 | 32 | virtual ~OpenSotSubtaskAdapter() override = default; 33 | 34 | private: 35 | 36 | SubtaskSoT::Ptr _sot_subtask; 37 | Subtask::Ptr _ci_subtask; 38 | OpenSotTaskAdapter::Ptr _task_adapter; 39 | }; 40 | 41 | } } 42 | 43 | #endif // OPENSOTSUBTASK_H 44 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotCom.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTCOM_H 2 | #define OPENSOTCOM_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | 11 | using ComSoT = OpenSoT::tasks::velocity::CoM; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotComAdapter : 16 | public OpenSotTaskAdapter, 17 | public virtual CartesianTaskObserver 18 | { 19 | 20 | public: 21 | 22 | OpenSotComAdapter(TaskDescription::Ptr task, 23 | Context::ConstPtr context); 24 | 25 | virtual TaskPtr constructTask() override; 26 | 27 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 28 | 29 | virtual void update(double time, double period) override; 30 | 31 | bool onBaseLinkChanged() override; 32 | 33 | bool onControlModeChanged() override; 34 | 35 | virtual ~OpenSotComAdapter() override = default; 36 | 37 | protected: 38 | 39 | private: 40 | 41 | ComSoT::Ptr _opensot_com; 42 | CartesianTask::Ptr _ci_com; 43 | }; 44 | 45 | } } 46 | #endif // OPENSOTCOM_H 47 | -------------------------------------------------------------------------------- /examples/plugin/manipulability/src/OpenSotManipulability.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTManipulability_H 2 | #define OPENSOTManipulability_H 3 | 4 | #include "Manipulability.h" 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | using ManipulabilitySoT = OpenSoT::tasks::velocity::Manipulability; 11 | using TaskSoT = OpenSoT::Task::TaskPtr; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | 16 | class OpenSotManipulability : public OpenSotTaskAdapter 17 | { 18 | 19 | public: 20 | 21 | OpenSotManipulability(TaskDescription::Ptr task, 22 | Context::ConstPtr context); 23 | 24 | virtual TaskPtr constructTask() override; 25 | 26 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 27 | 28 | virtual void update(double time, double period) override; 29 | 30 | virtual ~OpenSotManipulability() override = default; 31 | 32 | private: 33 | 34 | ManipulabilitySoT::Ptr _sot_task_manip; 35 | Manipulability::Ptr _ci_task; 36 | TaskSoT _sot_task; 37 | 38 | 39 | }; 40 | 41 | } } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /docs/source/tasks/cartesian.rst: -------------------------------------------------------------------------------- 1 | Cartesian task 2 | ============== 3 | A *Cartesian* task is specified by the ``type: Cartesian`` property. Its parameters 4 | are described below. 5 | 6 | distal_link 7 | ^^^^^^^^^^^ 8 | specifies the robot link to be controlled (mandatory, must be a valid robot link) 9 | 10 | base_link 11 | ^^^^^^^^^ 12 | specifies the robot link w.r.t. which the ``distal_link`` is controlled 13 | (optional, defaults to ``world``) 14 | 15 | use_local_subtasks 16 | ^^^^^^^^^^^^^^^^^^ 17 | selects whether subtasks are defined w.r.t. the distal_link 18 | local frame, or w.r.t. the global (base_link) frame (optional, defaults to ``false``) 19 | 20 | orientation_gain 21 | ^^^^^^^^^^^^^^^^ 22 | specifies the ratio between the orientation and position feedback; 23 | for instance, a value of ``orientation_gain: 2.0`` means that an orientation error of 24 | 2 rad is recovered in the same time as a position error of 1 m. 25 | 26 | Example 27 | ------- 28 | 29 | .. code-block:: yaml 30 | 31 | MyCartesianTask: 32 | name: left_arm 33 | type: Cartesian 34 | distal_link: arm1_8 35 | base_link: pelvis 36 | use_local_subtasks: true 37 | orientation_gain: 5.0 38 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotPostural.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTPOSTURAL_H 2 | #define OPENSOTPOSTURAL_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | 11 | using PosturalSoT = OpenSoT::tasks::velocity::Postural; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotPosturalAdapter : 16 | public OpenSotTaskAdapter 17 | { 18 | 19 | public: 20 | 21 | OpenSotPosturalAdapter(TaskDescription::Ptr task, 22 | Context::ConstPtr context); 23 | 24 | virtual TaskPtr constructTask() override; 25 | 26 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 27 | 28 | virtual void update(double time, double period) override; 29 | 30 | virtual ~OpenSotPosturalAdapter() override = default; 31 | 32 | protected: 33 | 34 | private: 35 | 36 | PosturalSoT::Ptr _opensot_postural; 37 | PosturalTask::Ptr _ci_postural; 38 | 39 | bool _use_inertia_matrix; 40 | Eigen::MatrixXd _inertia_matrix; 41 | Eigen::VectorXd _qref, _qdotref; 42 | }; 43 | 44 | } } 45 | #endif // OPENSOTPOSTURAL_H 46 | -------------------------------------------------------------------------------- /bindings/python/pyImpedance.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace py = pybind11; 11 | 12 | using namespace XBot::Cartesian; 13 | 14 | std::string impedance_repr(const Impedance& imp) 15 | { 16 | std::stringstream ss; 17 | ss << "stiffness:\n" << imp.stiffness; 18 | ss << "\ndamping :\n" << imp.damping; 19 | ss << "\nmass :\n" << imp.mass; 20 | return ss.str(); 21 | } 22 | 23 | PYBIND11_MODULE(impedance, m) { 24 | 25 | py::class_(m, "Impedance") 26 | .def(py::init(), 27 | py::arg("stiffness") = Eigen::Matrix6d::Zero(), 28 | py::arg("damping") = Eigen::Matrix6d::Zero(), 29 | py::arg("mass") = Eigen::Matrix6d::Identity()) 30 | .def_readwrite("stiffness", &Impedance::stiffness) 31 | .def_readwrite("damping", &Impedance::damping) 32 | .def_readwrite("mass", &Impedance::mass) 33 | .def("__repr__", impedance_repr); 34 | } 35 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/Subtask.h: -------------------------------------------------------------------------------- 1 | #ifndef SUBTASK_IMPL_H 2 | #define SUBTASK_IMPL_H 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | 10 | class SubtaskImpl : public virtual Subtask, 11 | public TaskDescriptionImpl 12 | { 13 | 14 | public: 15 | 16 | 17 | SubtaskImpl(TaskDescriptionImpl::Ptr task, 18 | std::vector indices, 19 | std::string subtask_name = ""); 20 | 21 | 22 | TaskDescription::Ptr getTask() override; 23 | 24 | const Eigen::MatrixXd& getWeight() const override; 25 | bool setWeight(const Eigen::MatrixXd& value) override; 26 | 27 | const std::vector& getSubtaskIndices() const override; 28 | 29 | void reset() override; 30 | 31 | private: 32 | 33 | static std::string gen_subtask_name(TaskDescriptionImpl::Ptr task, 34 | std::vector indices); 35 | 36 | TaskDescriptionImpl::Ptr _task; 37 | 38 | std::vector _indices; 39 | 40 | mutable Eigen::MatrixXd _tmp_weight_small, _tmp_weight_big; 41 | }; 42 | 43 | } } 44 | 45 | #endif // SUBTASK_H 46 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotVelocityLimits.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTVELOCITYLIMITS_H 2 | #define OPENSOTVELOCITYLIMITS_H 3 | 4 | 5 | #include 6 | 7 | #include 8 | #include "OpenSotTask.h" 9 | 10 | #include 11 | 12 | using VelocityLimitsSoT = OpenSoT::constraints::velocity::VelocityLimits; 13 | 14 | namespace XBot { namespace Cartesian { 15 | 16 | class OpenSotVelocityLimitsAdapter : 17 | public OpenSotConstraintAdapter 18 | { 19 | 20 | public: 21 | 22 | OpenSotVelocityLimitsAdapter(ConstraintDescription::Ptr constr, 23 | Context::ConstPtr context); 24 | 25 | virtual ConstraintPtr constructConstraint() override; 26 | 27 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 28 | 29 | virtual void update(double time, double period) override; 30 | 31 | virtual ~OpenSotVelocityLimitsAdapter() override = default; 32 | 33 | protected: 34 | 35 | private: 36 | 37 | VelocityLimitsSoT::Ptr _opensot_vlim; 38 | VelocityLimits::Ptr _ci_vlim; 39 | }; 40 | 41 | 42 | 43 | 44 | } } 45 | 46 | #endif // OPENSOTVELOCITYLIMITS_H 47 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Limits.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_LIMITS_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_LIMITS_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | class JointLimits : public virtual ConstraintDescription 10 | { 11 | 12 | public: 13 | 14 | CARTESIO_DECLARE_SMART_PTR(JointLimits) 15 | 16 | virtual bool setBoundScaling(double value) = 0; 17 | virtual double getBoundScaling() const = 0; 18 | 19 | virtual Eigen::VectorXd getQmin() const = 0; 20 | virtual Eigen::VectorXd getQmax() const = 0; 21 | 22 | }; 23 | 24 | class JointLimitsInvariance : public JointLimits 25 | { 26 | public: 27 | 28 | CARTESIO_DECLARE_SMART_PTR(JointLimitsInvariance) 29 | 30 | virtual Eigen::VectorXd getQddotMax() const = 0; 31 | 32 | }; 33 | 34 | class VelocityLimits : public virtual ConstraintDescription 35 | { 36 | 37 | public: 38 | 39 | CARTESIO_DECLARE_SMART_PTR(VelocityLimits) 40 | 41 | virtual Eigen::VectorXd getQdotMax() const = 0; 42 | 43 | }; 44 | 45 | } } 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotGaze.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTGAZEADAPTER_H 2 | #define OPENSOTGAZEADAPTER_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | 11 | using GazeSoT = OpenSoT::tasks::velocity::Gaze; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotGazeAdapter : 16 | public OpenSotTaskAdapter, 17 | public virtual CartesianTaskObserver 18 | { 19 | 20 | public: 21 | 22 | OpenSotGazeAdapter(TaskDescription::Ptr task, 23 | Context::ConstPtr context); 24 | 25 | virtual TaskPtr constructTask() override; 26 | 27 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 28 | 29 | virtual void update(double time, double period) override; 30 | 31 | bool onBaseLinkChanged() override; 32 | 33 | bool onControlModeChanged() override; 34 | 35 | virtual ~OpenSotGazeAdapter() override = default; 36 | 37 | protected: 38 | 39 | GazeSoT::Ptr _opensot_cart; 40 | 41 | private: 42 | 43 | GazeTask::Ptr _ci_cart; 44 | }; 45 | 46 | 47 | 48 | 49 | } } 50 | 51 | #endif // OPENSOTTASKADAPTER_H 52 | -------------------------------------------------------------------------------- /include/cartesian_interface/utils/AccMaxComputer.h: -------------------------------------------------------------------------------- 1 | #ifndef __CI_ACCMAXCOMPUTER_H__ 2 | #define __CI_ACCMAXCOMPUTER_H__ 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { namespace Utils{ 7 | 8 | typedef Eigen::VectorXd QDDotMin; 9 | typedef Eigen::VectorXd QDDotMax; 10 | 11 | class AccMaxComputer 12 | { 13 | public: 14 | /** 15 | * @brief computeAccMax computes an estimation of the maximum joint accelerations based on maximum joint velocities 16 | * and torques. It is based on the procedure described in "Joint Position and Velocity Bounds in Discrete-Time 17 | * Acceleration/Torque Control of Robot Manipulators" by Andrea del Prete 18 | * @param model of the robot 19 | * @param configs number of random configs used 20 | * @param initial_qddotmax initial value for maximum joint accelerations 21 | * @return pair of (qddotmin, qddotmax) 22 | */ 23 | static std::pair computeAccMax(XBot::ModelInterface::Ptr model, 24 | int configs = 1e4, 25 | double initial_qddotmax = 1e4); 26 | }; 27 | 28 | 29 | }}} 30 | 31 | #endif 32 | 33 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotCartesian.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTCARTESIANADAPTER_H 2 | #define OPENSOTCARTESIANADAPTER_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | 11 | using CartesianSoT = OpenSoT::tasks::velocity::Cartesian; 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotCartesianAdapter : 16 | public OpenSotTaskAdapter, 17 | public virtual CartesianTaskObserver 18 | { 19 | 20 | public: 21 | 22 | OpenSotCartesianAdapter(TaskDescription::Ptr task, 23 | Context::ConstPtr context); 24 | 25 | virtual TaskPtr constructTask() override; 26 | 27 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 28 | 29 | virtual void update(double time, double period) override; 30 | 31 | bool onBaseLinkChanged() override; 32 | 33 | bool onControlModeChanged() override; 34 | 35 | virtual ~OpenSotCartesianAdapter() override = default; 36 | 37 | protected: 38 | 39 | CartesianSoT::Ptr _opensot_cart; 40 | 41 | private: 42 | 43 | CartesianTask::Ptr _ci_cart; 44 | }; 45 | 46 | 47 | 48 | 49 | } } 50 | 51 | #endif // OPENSOTTASKADAPTER_H 52 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotConstraintFromTask.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTCONSTRAINTFROMTASK_H 2 | #define OPENSOTCONSTRAINTFROMTASK_H 3 | 4 | 5 | #include 6 | 7 | #include 8 | #include "OpenSotTask.h" 9 | 10 | #include 11 | 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotConstraintFromTaskAdapter : 16 | public OpenSotConstraintAdapter 17 | { 18 | 19 | public: 20 | 21 | OpenSotConstraintFromTaskAdapter(ConstraintDescription::Ptr constr, 22 | Context::ConstPtr context); 23 | 24 | OpenSoT::OptvarHelper::VariableVector getRequiredVariables() const override; 25 | 26 | virtual ConstraintPtr constructConstraint() override; 27 | 28 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 29 | 30 | virtual void update(double time, double period) override; 31 | 32 | void processSolution(const Eigen::VectorXd &solution) override; 33 | 34 | virtual ~OpenSotConstraintFromTaskAdapter() override = default; 35 | 36 | protected: 37 | 38 | private: 39 | 40 | OpenSotTaskAdapter::Ptr _task_adapter; 41 | 42 | }; 43 | 44 | 45 | } } 46 | 47 | #endif // OPENSOTCONSTRAINTFROMTASK_H 48 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotInteraction.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTINTERACTION_H 2 | #define OPENSOTINTERACTION_H 3 | 4 | #include "OpenSotCartesian.h" 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | using AdmittanceSoT = OpenSoT::tasks::velocity::CartesianAdmittance; 14 | 15 | namespace XBot { namespace Cartesian { 16 | 17 | class OpenSotInteractionAdapter : 18 | public OpenSotCartesianAdapter 19 | { 20 | 21 | public: 22 | 23 | OpenSotInteractionAdapter(TaskDescription::Ptr task, 24 | Context::ConstPtr context); 25 | 26 | virtual TaskPtr constructTask() override; 27 | 28 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 29 | 30 | virtual void update(double time, double period) override; 31 | 32 | protected: 33 | 34 | AdmittanceSoT::Ptr _opensot_adm; 35 | 36 | private: 37 | 38 | AdmittanceTask::Ptr _ci_adm; 39 | bool _fest_upd; 40 | Utils::ForceEstimation::Ptr _fest; 41 | 42 | static Utils::ForceEstimation::WeakPtr _fest_weak; 43 | 44 | }; 45 | 46 | } } 47 | 48 | #endif // OPENSOTINTERACTION_H 49 | -------------------------------------------------------------------------------- /include/cartesian_interface/Context.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTEXT_H 2 | #define CONTEXT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace XBot { namespace Cartesian { 10 | 11 | class ProblemDescription; 12 | 13 | class Parameters 14 | { 15 | 16 | public: 17 | 18 | CARTESIO_DECLARE_SMART_PTR(Parameters) 19 | 20 | Parameters(double dt, 21 | std::string log_path = "/tmp"); 22 | 23 | double getControlPeriod() const; 24 | std::string getLogPath() const; 25 | 26 | bool isLogEnabled() const; 27 | void setLogEnabled(bool flag); 28 | 29 | private: 30 | 31 | double _dt; 32 | std::string _log_path; 33 | bool _enable_log; 34 | 35 | }; 36 | 37 | class Context 38 | { 39 | 40 | public: 41 | 42 | CARTESIO_DECLARE_SMART_PTR(Context) 43 | 44 | Context(Parameters::Ptr params, 45 | ModelInterface::Ptr model); 46 | 47 | Context(Parameters::Ptr params); 48 | 49 | Parameters::Ptr params(); 50 | Parameters::ConstPtr params() const; 51 | 52 | ModelInterface::Ptr model(); 53 | ModelInterface::ConstPtr model() const; 54 | 55 | 56 | private: 57 | 58 | Parameters::Ptr _params; 59 | ModelInterface::Ptr _model; 60 | 61 | }; 62 | 63 | 64 | } } 65 | 66 | 67 | #endif // CONTEXT_H 68 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/Constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_CONSTRAINT_IMPL_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_CONSTRAINT_IMPL_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | 10 | struct ConstraintFromTaskImpl : public virtual ConstraintFromTask, 11 | public TaskDescriptionImpl 12 | { 13 | 14 | CARTESIO_DECLARE_SMART_PTR(ConstraintFromTaskImpl) 15 | 16 | ConstraintFromTaskImpl() = default; 17 | ConstraintFromTaskImpl(TaskDescriptionImpl::Ptr task); 18 | 19 | bool validate() override; 20 | void update(double time, double period) override; 21 | void reset() override; 22 | void log(MatLogger2::Ptr logger, bool init_logger, int buf_size) override; 23 | bool getTaskError(Eigen::VectorXd &e) const override; 24 | 25 | TaskDescription::Ptr getTask() override; 26 | 27 | private: 28 | 29 | TaskDescriptionImpl::Ptr _task; 30 | 31 | }; 32 | 33 | ConstraintDescription::Ptr MakeConstraintFromTask(TaskDescription::Ptr task); 34 | 35 | TaskDescription::Ptr GetTaskFromConstraint(ConstraintDescription::Ptr constr); 36 | 37 | } } 38 | 39 | 40 | 41 | 42 | 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/SolverPlugin.h: -------------------------------------------------------------------------------- 1 | #ifndef SOLVERPLUGIN_H 2 | #define SOLVERPLUGIN_H 3 | 4 | #define CARTESIO_REGISTER_SOLVER_PLUGIN(ClassName, SolverName) \ 5 | extern "C" ::XBot::Cartesian::CartesianInterfaceImpl* \ 6 | create_cartesio_##SolverName##_solver(::XBot::Cartesian::ProblemDescription ik_pb, \ 7 | ::XBot::Cartesian::Context::Ptr context, \ 8 | ::XBot::Cartesian::detail::Version ci_ver) \ 9 | { \ 10 | ::XBot::Cartesian::detail::Version plugin_ver CARTESIO_ABI_VERSION; \ 11 | \ 12 | if(!plugin_ver.isCompatible(ci_ver)) \ 13 | { \ 14 | fprintf(stderr, \ 15 | "Incompatible plugin version %d.%d.%d (expected %d.%d.%d)", \ 16 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 17 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 18 | throw std::runtime_error("Version mismatch"); \ 19 | } \ 20 | \ 21 | if(!(ci_ver == plugin_ver)) \ 22 | { \ 23 | fprintf(stderr, \ 24 | "Different plugin version %d.%d.%d (expected %d.%d.%d)", \ 25 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 26 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 27 | } \ 28 | \ 29 | return new ClassName(ik_pb, context); \ 30 | } 31 | 32 | 33 | #endif // SOLVERPLUGIN_H 34 | -------------------------------------------------------------------------------- /src/Context.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace XBot::Cartesian; 5 | 6 | Parameters::Parameters(double dt, 7 | std::string log_path): 8 | _dt(dt), 9 | _log_path(log_path), 10 | _enable_log(true) 11 | { 12 | 13 | } 14 | 15 | bool Parameters::isLogEnabled() const 16 | { 17 | return _enable_log; 18 | } 19 | 20 | void Parameters::setLogEnabled(bool flag) 21 | { 22 | _enable_log = flag; 23 | } 24 | 25 | double Parameters::getControlPeriod() const 26 | { 27 | return _dt; 28 | } 29 | 30 | std::string Parameters::getLogPath() const 31 | { 32 | return _log_path; 33 | } 34 | 35 | Context::Context(Parameters::Ptr params, 36 | ModelInterface::Ptr model): 37 | _params(params), 38 | _model(model) 39 | { 40 | if(!model) 41 | { 42 | throw std::invalid_argument("Model is nullptr"); 43 | } 44 | } 45 | 46 | Context::Context(Parameters::Ptr params): 47 | _params(params) 48 | { 49 | 50 | } 51 | 52 | Parameters::Ptr Context::params() 53 | { 54 | return _params; 55 | } 56 | 57 | Parameters::ConstPtr Context::params() const 58 | { 59 | return _params; 60 | } 61 | 62 | XBot::ModelInterface::Ptr Context::model() 63 | { 64 | return _model; 65 | } 66 | 67 | XBot::ModelInterface::ConstPtr Context::model() const 68 | { 69 | return _model; 70 | } 71 | -------------------------------------------------------------------------------- /src/utils/RobotStatePublisher.cpp: -------------------------------------------------------------------------------- 1 | #include "cartesian_interface/utils/RobotStatePublisher.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { namespace Utils { 7 | 8 | RobotStatePublisher::RobotStatePublisher(ModelInterface::ConstPtr model): 9 | _model(model) 10 | { 11 | 12 | } 13 | 14 | void RobotStatePublisher::publishTransforms(const ros::Time & time, 15 | const std::string & tf_prefix) 16 | { 17 | const auto& urdf = *_model->getUrdf(); 18 | 19 | _tf_vector.clear(); 20 | 21 | for(const auto& jmap : urdf.joints_) 22 | { 23 | const auto& jname = jmap.first; 24 | const auto& joint = jmap.second; 25 | 26 | const auto& parent_link = joint->parent_link_name; 27 | const auto& child_link = joint->child_link_name; 28 | 29 | Eigen::Affine3d T; 30 | _model->getPose(child_link, parent_link, T); 31 | 32 | geometry_msgs::TransformStamped tf; 33 | tf::transformEigenToMsg(T, tf.transform); 34 | tf.header.stamp = time; 35 | tf.header.frame_id = tf_prefix + "/" + parent_link; 36 | tf.child_frame_id = tf_prefix + "/" + child_link; 37 | 38 | _tf_vector.push_back(tf); 39 | } 40 | 41 | _tf_broadcaster.sendTransform(_tf_vector); 42 | 43 | } 44 | 45 | } } } 46 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_CONSTRAINT_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_CONSTRAINT_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace XBot { namespace Cartesian { 9 | 10 | 11 | /** 12 | * @brief Base class for the description of a constraint. 13 | */ 14 | struct ConstraintDescription : public virtual TaskDescription 15 | { 16 | typedef std::shared_ptr Ptr; 17 | typedef std::shared_ptr ConstPtr; 18 | 19 | static bool IsConstraint(TaskDescription::ConstPtr task); 20 | static Ptr AsConstraint(TaskDescription::Ptr task); 21 | static ConstPtr AsConstraint(TaskDescription::ConstPtr task); 22 | 23 | }; 24 | 25 | struct ConstraintFromTask : public virtual ConstraintDescription 26 | { 27 | typedef std::shared_ptr Ptr; 28 | typedef std::shared_ptr ConstPtr; 29 | 30 | virtual TaskDescription::Ptr getTask() = 0; 31 | }; 32 | 33 | ConstraintDescription::Ptr MakeConstraintFromTask(TaskDescription::Ptr task); 34 | 35 | TaskDescription::Ptr GetTaskFromConstraint(ConstraintDescription::Ptr constr); 36 | 37 | } } 38 | 39 | 40 | 41 | 42 | 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/problem/impl/OmniWheels4X.cpp: -------------------------------------------------------------------------------- 1 | #include "problem/OmniWheels4X.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OmniWheels4X::OmniWheels4X(YAML::Node yaml, Context::ConstPtr context): 6 | TaskDescriptionImpl(yaml, 7 | context, 8 | "OmniWheels4X", 9 | context->model()->getJointNum()), 10 | _base_link("base_link") 11 | { 12 | if(yaml["l1"]) 13 | { 14 | _l1 = yaml["l1"].as(); 15 | } 16 | else 17 | throw std::invalid_argument("Missing mandatory l1 argument!"); 18 | 19 | if(yaml["l2"]) 20 | { 21 | _l2 = yaml["l2"].as(); 22 | } 23 | else 24 | throw std::invalid_argument("Missing mandatory l2 argument!"); 25 | 26 | if(yaml["wheel_radius"]) 27 | { 28 | _r = yaml["wheel_radius"].as(); 29 | } 30 | else 31 | throw std::invalid_argument("Missing mandatory wheel_radius argument!"); 32 | 33 | if(yaml["base_link"]) 34 | { 35 | _base_link = yaml["base_link"].as(); 36 | } 37 | 38 | if(yaml["joint_wheels_name"]) 39 | { 40 | for(auto joint_name : yaml["joint_wheels_name"]) 41 | _joint_wheels_name.push_back(joint_name.as()); 42 | } 43 | else 44 | throw std::invalid_argument("Missing mandatory joint_wheels_name argument!"); 45 | 46 | 47 | } 48 | -------------------------------------------------------------------------------- /include/cartesian_interface/Macro.h: -------------------------------------------------------------------------------- 1 | #ifndef MACRO_H 2 | #define MACRO_H 3 | 4 | namespace XBot { namespace Cartesian { namespace detail { 5 | 6 | struct Version 7 | { 8 | int major = -1; 9 | int minor = -1; 10 | int patch = -1; 11 | 12 | Version(int a_major, int a_minor, int a_patch) 13 | { 14 | major = a_major; 15 | minor = a_minor; 16 | patch = a_patch; 17 | } 18 | 19 | bool isCompatible(Version other) 20 | { 21 | return major == other.major && minor == other.minor; 22 | } 23 | }; 24 | 25 | inline bool operator==(Version a, Version b) 26 | { 27 | return a.major == b.major && 28 | a.minor == b.minor && 29 | a.patch == b.patch; 30 | } 31 | 32 | } } } 33 | 34 | #define CARTESIO_ABI_VERSION (2, 1, 0) 35 | 36 | #define CARTESIO_DECLARE_SMART_PTR(Class) \ 37 | typedef std::shared_ptr Ptr; \ 38 | typedef std::shared_ptr ConstPtr; \ 39 | typedef std::weak_ptr WeakPtr; \ 40 | typedef std::unique_ptr UniquePtr; \ 41 | 42 | #define NOTIFY_OBSERVERS(FieldName) \ 43 | bool notify_success = true; \ 44 | for(auto obs_weak: _observers) \ 45 | { \ 46 | if(auto obs = obs_weak.lock()) \ 47 | { \ 48 | if(!obs->on##FieldName##Changed()) notify_success = false; \ 49 | } \ 50 | } 51 | 52 | 53 | 54 | 55 | 56 | 57 | #endif // MACRO_H 58 | -------------------------------------------------------------------------------- /src/xbot2/cartesioplugin.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTESIO_RT_H 2 | #define CARTESIO_RT_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace XBot { 12 | 13 | class CartesioRt : public ControlPlugin 14 | { 15 | 16 | public: 17 | 18 | using ControlPlugin::ControlPlugin; 19 | 20 | bool on_initialize() override; 21 | void starting() override; 22 | void run() override; 23 | void stopping() override; 24 | void on_abort() override; 25 | void on_close() override; 26 | 27 | private: 28 | 29 | std::unique_ptr _nh; 30 | 31 | bool _enable_feedback; 32 | 33 | Eigen::VectorXd _q, _qdot; 34 | ModelInterface::Ptr _rt_model; 35 | Cartesian::CartesianInterfaceImpl::Ptr _rt_ci; 36 | Cartesian::LockfreeBufferImpl::Ptr _nrt_ci; 37 | std::atomic_bool _rt_active; 38 | std::atomic_bool _nrt_exit; 39 | double _fake_time; 40 | 41 | std::unique_ptr _nrt_th; 42 | 43 | Hal::LinkStateSensor::Ptr _lss; 44 | 45 | bool init_load_world_frame(); 46 | 47 | Eigen::Affine3d _fb_T_l, _l_T_m; 48 | 49 | Eigen::VectorXd _zeros, _initial_stiffness, _initial_damping; 50 | }; 51 | 52 | 53 | } 54 | 55 | #endif // CARTESIO_RT_H 56 | -------------------------------------------------------------------------------- /src/plugin/angular_mom/OpenSotAngularMomentum.cpp: -------------------------------------------------------------------------------- 1 | #include "OpenSotAngularMomentum.h" 2 | 3 | #include 4 | 5 | #include "fmt/format.h" 6 | 7 | using namespace XBot::Cartesian; 8 | 9 | OpenSotAngularMomentum::OpenSotAngularMomentum(TaskDescription::Ptr task, 10 | ModelInterface::ConstPtr model): 11 | OpenSotTaskAdapter(task, model) 12 | { 13 | _ci_angmom = std::dynamic_pointer_cast(task); 14 | 15 | if(!_ci_angmom) throw std::runtime_error("Provided task description " 16 | "does not have expected type 'AngularMomentum'"); 17 | 18 | } 19 | 20 | TaskPtr OpenSotAngularMomentum::constructTask() 21 | { 22 | Eigen::VectorXd q; 23 | _model->getJointPosition(q); 24 | 25 | _sot_angmom = SotUtils::make_shared(q, 26 | const_cast(*_model)); 27 | 28 | return _sot_angmom; 29 | } 30 | 31 | bool OpenSotAngularMomentum::initialize() 32 | { 33 | return OpenSotTaskAdapter::initialize(); 34 | } 35 | 36 | void OpenSotAngularMomentum::update(double time, double period) 37 | { 38 | OpenSotTaskAdapter::update(time, period); 39 | 40 | _sot_angmom->setReference(_ci_angmom->getReference() * period); 41 | } 42 | 43 | CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(OpenSotAngularMomentum) 44 | 45 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/Plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef PLUGIN_TASK_H 2 | #define PLUGIN_TASK_H 3 | 4 | // create_cartesian_interface_task_description 5 | #define CARTESIO_REGISTER_TASK_PLUGIN(ClassName, TaskType) \ 6 | extern "C" ::XBot::Cartesian::TaskDescription* create_cartesio_##TaskType##_description(YAML::Node task_node, \ 7 | ::XBot::Cartesian::Context::ConstPtr context, \ 8 | ::XBot::Cartesian::detail::Version ci_ver) \ 9 | { \ 10 | ::XBot::Cartesian::detail::Version plugin_ver CARTESIO_ABI_VERSION; \ 11 | \ 12 | if(!plugin_ver.isCompatible(ci_ver)) \ 13 | { \ 14 | fprintf(stderr, \ 15 | "Incompatible plugin version %d.%d.%d (expected %d.%d.%d)", \ 16 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 17 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 18 | throw std::runtime_error("Version mismatch"); \ 19 | } \ 20 | \ 21 | if(!(ci_ver == plugin_ver)) \ 22 | { \ 23 | fprintf(stderr, \ 24 | "Different plugin version %d.%d.%d (expected %d.%d.%d)", \ 25 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 26 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 27 | } \ 28 | \ 29 | return new ClassName(task_node, context); \ 30 | } 31 | 32 | #endif // PLUGIN_H 33 | -------------------------------------------------------------------------------- /examples/configs/coman_stack_alternative.yaml: -------------------------------------------------------------------------------- 1 | solver_options: 2 | regularization: 1e-3 3 | back_end: "qpoases" #"osqp" 4 | #front_end: "hcod" 5 | #disable_weights_computation: False 6 | 7 | stack: 8 | - ["LeftFoot", "RightFoot"] 9 | - ["ComXY"] 10 | - ["LeftArm", "RightArm"] 11 | - ["Postural", "MinEffort"] 12 | 13 | constraints: ["JointLimits", "VelocityLimits"] 14 | 15 | LeftFoot: 16 | type: "Cartesian" 17 | name: "left_foot" 18 | distal_link: "l_sole" 19 | 20 | RightFoot: 21 | type: "Cartesian" 22 | name: "right_foot" 23 | distal_link: "r_sole" 24 | 25 | LeftArm: 26 | type: "Cartesian" 27 | name: "left_hand" 28 | distal_link: "LSoftHand" 29 | lambda: 0.1 30 | weight: 10. 31 | 32 | RightArm: 33 | type: "Cartesian" 34 | name: "right_hand" 35 | distal_link: "RSoftHand" 36 | lambda: 0.1 37 | 38 | Postural: 39 | type: "Postural" 40 | lambda: 0.01 41 | weight: 42 | reference@v0: 0.0 43 | reference@v1: 0.0 44 | reference@v2: 0.0 45 | reference@v3: 0.0 46 | reference@v4: 0.0 47 | reference@v5: 0.0 48 | 49 | MinEffort: 50 | type: MinimumEffort 51 | lib_name: libCartesioMinimumEffortAddon.so 52 | 53 | ComXY: 54 | type: "Com" 55 | lambda: 0.1 56 | indices: [0, 1] 57 | weight: 10 58 | 59 | JointLimits: 60 | type: "JointLimits" 61 | 62 | 63 | VelocityLimits: 64 | type: "VelocityLimits" 65 | -------------------------------------------------------------------------------- /bindings/python/pyRosInit.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | bool init(std::string name, std::list args) 7 | { 8 | if(ros::ok()) 9 | { 10 | ROS_ERROR("Ros node already initialized with name %s", 11 | ros::this_node::getName().c_str()); 12 | return false; 13 | } 14 | 15 | std::vector args_vec; 16 | for(auto& a : args) 17 | { 18 | args_vec.push_back(a.c_str()); 19 | } 20 | 21 | int argc = args_vec.size(); 22 | 23 | char ** argv = (char **)args_vec.data(); 24 | 25 | name += "_cpp"; 26 | 27 | ros::init(argc, argv, name, ros::init_options::NoSigintHandler); 28 | 29 | ROS_INFO("Initialized roscpp under namespace %s with name %s", 30 | ros::this_node::getNamespace().c_str(), 31 | ros::this_node::getName().c_str() 32 | ); 33 | 34 | return true; 35 | } 36 | 37 | bool shutdown() 38 | { 39 | if(ros::ok()) 40 | { 41 | ROS_INFO("Shutting down ros node"); 42 | ros::shutdown(); 43 | return true; 44 | } 45 | 46 | std::cerr << "Ros node not running" << std::endl; 47 | return false; 48 | } 49 | 50 | 51 | namespace py = pybind11; 52 | 53 | PYBIND11_MODULE(roscpp_utils, m) { 54 | 55 | m.def("init", init); 56 | m.def("shutdown", shutdown); 57 | 58 | } -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotVelocityLimits.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotVelocityLimits.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotVelocityLimitsAdapter::OpenSotVelocityLimitsAdapter(ConstraintDescription::Ptr constr, 6 | Context::ConstPtr context): 7 | OpenSotConstraintAdapter (constr, context) 8 | { 9 | _ci_vlim = std::dynamic_pointer_cast(constr); 10 | 11 | if(!_ci_vlim) 12 | { 13 | throw std::runtime_error("Provided constraint description " 14 | "does not have expected type 'VelocityLimits'"); 15 | } 16 | 17 | } 18 | 19 | ConstraintPtr OpenSotVelocityLimitsAdapter::constructConstraint() 20 | { 21 | _opensot_vlim = SotUtils::make_shared(const_cast(*_model), _ci_vlim->getQdotMax(), 22 | _ctx->params()->getControlPeriod()); 23 | 24 | return _opensot_vlim; 25 | } 26 | 27 | bool OpenSotVelocityLimitsAdapter::initialize(const OpenSoT::OptvarHelper& vars) 28 | { 29 | if(vars.getAllVariables().size() > 0) 30 | { 31 | throw BadVariables("[OpenSotVelocityLimitsAdapter] requires default variables definition"); 32 | } 33 | 34 | return OpenSotConstraintAdapter::initialize(vars); 35 | } 36 | 37 | void OpenSotVelocityLimitsAdapter::update(double time, double period) 38 | { 39 | OpenSotConstraintAdapter::update(time, period); 40 | } 41 | -------------------------------------------------------------------------------- /tests/configs/centauro_test_stack.yaml: -------------------------------------------------------------------------------- 1 | solver_options: 2 | regularization: 1e-3 3 | back_end: "qpoases" 4 | 5 | stack: 6 | - ["Wheel_FR", "Wheel_HR", "Wheel_HL", "Wheel_FL"] 7 | - ["LeftArm", "RightArm", "ComXY"] 8 | - ["Postural", "ComZ"] 9 | constraints: ["JointLimits", "VelocityLimits"] 10 | 11 | JointLimits: 12 | type: "JointLimits" 13 | 14 | VelocityLimits: 15 | type: "VelocityLimits" 16 | 17 | Wheel_FL: 18 | type: "Cartesian" 19 | distal_link: "wheel_1" 20 | 21 | Wheel_FR: 22 | type: "Cartesian" 23 | distal_link: "wheel_2" 24 | 25 | Wheel_HL: 26 | type: "Cartesian" 27 | distal_link: "wheel_3" 28 | 29 | Wheel_HR: 30 | type: "Cartesian" 31 | distal_link: "wheel_4" 32 | 33 | LeftArm: 34 | type: "Cartesian" 35 | distal_link: "arm1_8" 36 | base_link: "pelvis" 37 | lambda: 0.1 38 | 39 | RightArm: 40 | type: "Cartesian" 41 | distal_link: "arm2_8" 42 | lambda: 0.1 43 | 44 | Postural: 45 | type: "Postural" 46 | lambda: 0.01 47 | weight: 48 | VIRTUALJOINT_1: 0.0 49 | VIRTUALJOINT_2: 0.0 50 | VIRTUALJOINT_3: 0.0 51 | VIRTUALJOINT_4: 0.0 52 | VIRTUALJOINT_5: 0.0 53 | VIRTUALJOINT_6: 0.0 54 | 55 | ComTask: 56 | type: "Com" 57 | weight: 10 58 | lambda: 0.05 59 | 60 | ComXY: 61 | type: "Subtask" 62 | task: "ComTask" 63 | indices: [0, 1] 64 | 65 | ComZ: 66 | type: "Subtask" 67 | task: "ComTask" 68 | indices: [2] 69 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/src/OpenSotAngularMomentum.cpp: -------------------------------------------------------------------------------- 1 | #include "OpenSotAngularMomentum.h" 2 | 3 | #include 4 | 5 | #include "fmt/format.h" 6 | 7 | using namespace XBot::Cartesian; 8 | 9 | OpenSotAngularMomentum::OpenSotAngularMomentum(TaskDescription::Ptr task, 10 | Context::ConstPtr context): 11 | OpenSotTaskAdapter(task, context) 12 | { 13 | _ci_angmom = std::dynamic_pointer_cast(task); 14 | 15 | if(!_ci_angmom) throw std::runtime_error("Provided task description " 16 | "does not have expected type 'AngularMomentum'"); 17 | 18 | } 19 | 20 | TaskPtr OpenSotAngularMomentum::constructTask() 21 | { 22 | 23 | _sot_angmom = SotUtils::make_shared(const_cast(*_model)); 24 | 25 | return _sot_angmom; 26 | } 27 | 28 | bool OpenSotAngularMomentum::initialize(const ::OpenSoT::OptvarHelper& vars) 29 | { 30 | if(vars.getAllVariables().size() > 0) 31 | { 32 | throw BadVariables("[OpenSotAngularMomentum] requires default variables definition"); 33 | } 34 | 35 | return OpenSotTaskAdapter::initialize(vars); 36 | } 37 | 38 | void OpenSotAngularMomentum::update(double time, double period) 39 | { 40 | OpenSotTaskAdapter::update(time, period); 41 | 42 | _sot_angmom->setReference(_ci_angmom->getReference() * period); 43 | } 44 | 45 | CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(OpenSotAngularMomentum, AngularMomentum) 46 | 47 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/src/AngularMomentumRos.h: -------------------------------------------------------------------------------- 1 | #ifndef ANGULARMOMENTUMROS_H 2 | #define ANGULARMOMENTUMROS_H 3 | 4 | #include "AngularMomentum.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace XBot { namespace Cartesian { 10 | 11 | namespace ServerApi 12 | { 13 | class AngularMomentumRos; 14 | } 15 | 16 | /** 17 | * @brief The ServerApi::AngularMomentumRos class implements a ROS 18 | * interface for the task. 19 | */ 20 | class ServerApi::AngularMomentumRos : public ServerApi::TaskRos 21 | { 22 | 23 | public: 24 | 25 | 26 | AngularMomentumRos(TaskDescription::Ptr task, 27 | RosContext::Ptr ros_context); 28 | 29 | void run(ros::Time time) override; 30 | 31 | 32 | private: 33 | 34 | void on_ref_recv(geometry_msgs::Vector3StampedConstPtr msg); 35 | 36 | ros::Subscriber _ref_sub; 37 | ros::Publisher _cur_ref_pub; 38 | 39 | AngularMomentum::Ptr _ci_angmom; 40 | 41 | 42 | }; 43 | 44 | 45 | namespace ClientApi 46 | { 47 | class AngularMomentumRos; 48 | } 49 | 50 | class ClientApi::AngularMomentumRos : virtual public AngularMomentum, public ClientApi::TaskRos 51 | { 52 | 53 | public: 54 | 55 | CARTESIO_DECLARE_SMART_PTR(AngularMomentumRos) 56 | 57 | AngularMomentumRos(std::string name, 58 | ros::NodeHandle nh):TaskRos(name, nh) {} 59 | 60 | 61 | 62 | private: 63 | 64 | 65 | }; 66 | 67 | }} 68 | 69 | #endif // ANGULARMOMENTUMROS_H 70 | -------------------------------------------------------------------------------- /examples/plugin/min_effort/src/OpenSotMinEffort.cpp: -------------------------------------------------------------------------------- 1 | #include "OpenSotMinEffort.h" 2 | 3 | #include 4 | 5 | #include "fmt/format.h" 6 | 7 | using namespace XBot::Cartesian; 8 | 9 | OpenSotMinimumEffort::OpenSotMinimumEffort(TaskDescription::Ptr task, 10 | Context::ConstPtr context): 11 | OpenSotTaskAdapter(task, context) 12 | { 13 | _ci_task = std::dynamic_pointer_cast(task); 14 | 15 | if(!_ci_task) throw std::runtime_error("Provided task description " 16 | "does not have expected type 'MinimumEffort'"); 17 | 18 | } 19 | 20 | TaskPtr OpenSotMinimumEffort::constructTask() 21 | { 22 | 23 | _sot_task = SotUtils::make_shared(*_model, 24 | _ci_task->getStepSize() 25 | ); 26 | 27 | return _sot_task; 28 | } 29 | 30 | bool OpenSotMinimumEffort::initialize(const ::OpenSoT::OptvarHelper& vars) 31 | { 32 | if(vars.getAllVariables().size() > 0) 33 | { 34 | throw BadVariables("[OpenSotMinimumEffort] requires default variables definition"); 35 | } 36 | 37 | return OpenSotTaskAdapter::initialize(vars); 38 | } 39 | 40 | void OpenSotMinimumEffort::update(double time, double period) 41 | { 42 | OpenSotTaskAdapter::update(time, period); 43 | 44 | // any custom update action 45 | } 46 | 47 | CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(OpenSotMinimumEffort, MinimumEffort) 48 | 49 | -------------------------------------------------------------------------------- /examples/plugin/min_effort/src/MinEffort.h: -------------------------------------------------------------------------------- 1 | #ifndef MINEFFORT_H 2 | #define MINEFFORT_H 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief The MinimumEffort class models the abstract 10 | * interface for the task. 11 | * 12 | * NOTE: it is mandatory to inherit **virtually** from TaskDescription 13 | */ 14 | class MinimumEffort : public virtual TaskDescription 15 | { 16 | 17 | public: 18 | 19 | CARTESIO_DECLARE_SMART_PTR(MinimumEffort) 20 | 21 | /* Define the task API (pure virtual methods) */ 22 | 23 | virtual double getStepSize() const = 0; 24 | 25 | }; 26 | 27 | /** 28 | * @brief The MinimumEffortImpl class implements the abstract 29 | * interface 30 | */ 31 | class MinimumEffortImpl : public TaskDescriptionImpl, 32 | public virtual MinimumEffort 33 | { 34 | 35 | public: 36 | 37 | CARTESIO_DECLARE_SMART_PTR(MinimumEffortImpl) 38 | 39 | /* The task implementation constructor signature must be 40 | * as follows */ 41 | MinimumEffortImpl(YAML::Node task_node, 42 | Context::ConstPtr context); 43 | 44 | /* Implement the task API */ 45 | 46 | double getStepSize() const override; 47 | 48 | /* Customize update, reset and log */ 49 | void update(double time, double period) override; 50 | void reset() override; 51 | void log(MatLogger2::Ptr logger, bool init_logger, int buf_size) override; 52 | 53 | private: 54 | 55 | double _step_size; 56 | 57 | }; 58 | 59 | } } 60 | 61 | #endif // MinimumEffort_H 62 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/ros/client_api/PosturalRos.h: -------------------------------------------------------------------------------- 1 | #ifndef POSTURALROS__CLIENTAPI_H 2 | #define POSTURALROS__CLIENTAPI_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace XBot { namespace Cartesian { 10 | 11 | namespace ClientApi 12 | { 13 | class PosturalRos; 14 | } 15 | 16 | class ClientApi::PosturalRos : virtual public PosturalTask, 17 | public ClientApi::TaskRos 18 | { 19 | 20 | public: 21 | 22 | CARTESIO_DECLARE_SMART_PTR(PosturalRos) 23 | 24 | PosturalRos(std::string name, 25 | ros::NodeHandle nh); 26 | 27 | bool validate() override; 28 | 29 | bool useInertiaMatrixWeight() const override; 30 | 31 | void getReferencePosture(Eigen::VectorXd& qref) const override; 32 | 33 | void getReferencePosture(JointNameMap& qref) const override; 34 | 35 | void setReferencePosture(const JointNameMap& qref) override; 36 | 37 | void setReferencePosture(const Eigen::VectorXd& qref) override; 38 | 39 | void setReferenceVelocity(const JointNameMap& qdotref) override; 40 | 41 | void setReferenceVelocity(const Eigen::VectorXd& qdotref) override; 42 | 43 | void getReferenceVelocity(Eigen::VectorXd& qdotref) const override; 44 | 45 | private: 46 | 47 | void on_current_ref_recv(sensor_msgs::JointStateConstPtr msg); 48 | 49 | ros::Publisher _ref_pub; 50 | bool _curr_ref_recv; 51 | ros::Subscriber _current_ref_sub; 52 | 53 | JointNameMap _current_ref; 54 | 55 | }; 56 | 57 | } } 58 | 59 | #endif // POSTURALROS_H 60 | -------------------------------------------------------------------------------- /include/cartesian_interface/Enum.h: -------------------------------------------------------------------------------- 1 | #ifndef __CARTESIAN_INTERFACE_ENUM_H__ 2 | #define __CARTESIAN_INTERFACE_ENUM_H__ 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief The ActivationState enum 10 | */ 11 | enum class ActivationState { Enabled, Disabled }; 12 | 13 | /** 14 | * @brief Enum describing a state for each task. Available states are: 15 | * - State::Online: the task is following an online getPoseReference 16 | * - State::Reacing: the task is performing a point-to-point motion 17 | */ 18 | enum class State { Reaching, Online }; 19 | 20 | /** 21 | * @brief Enum describing a control mode for each task. Available values are: 22 | * - ControlType::Position: the task is following position references 23 | * - ControlType::Velocity: the task is following velocity references 24 | * - ControlType::Disabled: the task is disabled 25 | */ 26 | enum class ControlType { Position, Velocity }; 27 | 28 | template 29 | std::string EnumToString(T value); 30 | 31 | template 32 | T StringToEnum(const std::string& value); 33 | 34 | template <> 35 | std::string EnumToString(ActivationState value); 36 | 37 | template <> 38 | std::string EnumToString(ControlType value); 39 | 40 | template <> 41 | std::string EnumToString(State value); 42 | 43 | template <> 44 | ActivationState StringToEnum(const std::string& value); 45 | 46 | template <> 47 | ControlType StringToEnum(const std::string& value); 48 | 49 | template <> 50 | State StringToEnum(const std::string& value); 51 | 52 | } } 53 | 54 | #endif // __CARTESIAN_INTERFACE_ENUM_H__ 55 | -------------------------------------------------------------------------------- /docs/source/plugins.rst: -------------------------------------------------------------------------------- 1 | .. _Expand: 2 | 3 | How to expand CartesIO 4 | ====================== 5 | CartesIO supports only a limited set of tasks and constraints. These are usually enough for 6 | setting up most control problems; however, special tasks might be needed for implementing more 7 | "exotic" behaviors. For this reason, CastesIO is designed to be **highly customizable** at almost 8 | all its architectural layers; the user can implement: 9 | 10 | - a **new controller**, which replaces the OpenSot-based velocity level inverse kinematics. 11 | For instance, a custom solver can be implemented from scratch, but remaining compatible with all the 12 | CartesIO interfaces and utilities. New controllers can then dynamically loaded 13 | with the ``load_controller`` ROS service. 14 | 15 | - A **new task/constraint**, which is added to the library of supported CartesIO APIs. This way, we can 16 | a programmatic API, and a ROS interface to a new kind of task. 17 | 18 | - When a new task/constraint is developed, the **OpenSot support** can be quickly implemented as well, 19 | in order to have it inside the default OpenSot-based velocity level inverse kinematics controller. 20 | 21 | The header files in ```` can be conveniently employed to re-use existing componens 22 | of the CartesIO framework. For instance, a specialization of a ``Cartesian`` task 23 | (such as a center-of-mass stabilizer) does not need to re-implement the whole Cartesian task API. All it needs 24 | to do is to properly inherit from the proper classes. 25 | 26 | 27 | .. toctree:: 28 | :maxdepth: 1 29 | 30 | plugins_task 31 | plugins_ros 32 | plugins_sot 33 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotOmniWheels4X.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotOmniWheels4X.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotOmniWheels4XAdapter::OpenSotOmniWheels4XAdapter(ConstraintDescription::Ptr constr, Context::ConstPtr context): 6 | OpenSotConstraintAdapter(constr, context) 7 | { 8 | _ci_omniwheels4x = std::dynamic_pointer_cast(constr); 9 | 10 | if(!_ci_omniwheels4x) throw std::runtime_error("Provided task description " 11 | "does not have expected type 'OmniWheels4X'"); 12 | } 13 | 14 | ConstraintPtr OpenSotOmniWheels4XAdapter::constructConstraint() 15 | { 16 | _opensot_omniwheels4x = SotUtils::make_shared(_ci_omniwheels4x->getl1(), 17 | _ci_omniwheels4x->getl2(), 18 | _ci_omniwheels4x->getr(), 19 | _ci_omniwheels4x->get_joint_wheels_name(), 20 | _ci_omniwheels4x->get_base_link(), 21 | const_cast(*_model)); 22 | 23 | return _opensot_omniwheels4x; 24 | } 25 | 26 | bool OpenSotOmniWheels4XAdapter::initialize(const OpenSoT::OptvarHelper &vars) 27 | { 28 | if(vars.getAllVariables().size() > 0) 29 | { 30 | throw BadVariables("[OpenSotOmniWheels4XAdapter] requires default variables definition"); 31 | } 32 | 33 | bool ret = OpenSotConstraintAdapter::initialize(vars); 34 | if(!ret) return false; 35 | 36 | return true; 37 | } 38 | -------------------------------------------------------------------------------- /src/utils/DynamicLoading.h: -------------------------------------------------------------------------------- 1 | #ifndef DYNAMICLOADING_H 2 | #define DYNAMICLOADING_H 3 | 4 | #ifndef LOAD_OBJECT_H 5 | #define LOAD_OBJECT_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | struct LibNotFound : public std::exception 13 | { 14 | const char * what() const noexcept override 15 | { 16 | return dlerror(); 17 | } 18 | }; 19 | 20 | struct SymbolNotFound : public std::runtime_error 21 | { 22 | using runtime_error::runtime_error; 23 | }; 24 | 25 | template 26 | RetType CallFunction(std::string lib_name, 27 | std::string function_name, 28 | Args... args) 29 | { 30 | 31 | /* Try to open the provided library */ 32 | void * lib_handle = dlopen(lib_name.c_str(), RTLD_NOW); 33 | 34 | /* Not able to open so, report error */ 35 | if(!lib_handle) 36 | { 37 | throw LibNotFound(); 38 | } 39 | else 40 | { 41 | 42 | /* Typedef for the factory type */ 43 | typedef RetType (*FactoryType)(Args... args); 44 | 45 | /* Try to obtain the address of the factory */ 46 | FactoryType function = reinterpret_cast(dlsym(lib_handle, 47 | function_name.c_str()) 48 | ); 49 | 50 | const char * error = dlerror(); 51 | if(error != nullptr) 52 | { 53 | throw SymbolNotFound(error); 54 | } 55 | 56 | return function(args...); 57 | 58 | } 59 | 60 | } 61 | 62 | #endif // LOAD_OBJECT_H 63 | 64 | 65 | #endif // DYNAMICLOADING_H 66 | -------------------------------------------------------------------------------- /src/ros/AccMaxComputerNode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace XBot::Cartesian::Utils; 6 | 7 | int main(int argc, char ** argv) 8 | { 9 | // init ros, get handles 10 | ros::init(argc, argv, "acc_max_computer_node"); 11 | ros::NodeHandle nh("cartesian"); 12 | ros::NodeHandle nh_priv("~"); 13 | 14 | int configs = nh_priv.param("configs", 1e6); 15 | double initial_qddotmax = nh_priv.param("initial_qddotmax", 1e4); 16 | 17 | XBot::ModelInterface::Ptr model = XBot::ModelInterface::getModel(XBot::Utils::ConfigOptionsFromParamServer()); 18 | 19 | ROS_INFO("Initial qddotmax is %f [rad/sec^2]", initial_qddotmax); 20 | ROS_INFO("Computing max accelerations with %i configurations...", configs); 21 | std::pair qddot_limits = AccMaxComputer::computeAccMax(model, configs, initial_qddotmax); 22 | ROS_INFO("...computed!"); 23 | 24 | std::vector joint_names = model->getJointNames(); 25 | for(unsigned int i = 0; i < joint_names.size(); ++i) 26 | ROS_INFO_STREAM(joint_names[i]<<" qddot_min: "< map_acc_lims; 31 | for(unsigned int i = 0; i < joint_names.size(); ++i) 32 | { 33 | unsigned int id = model->getDofIndex(joint_names[i]); 34 | map_acc_lims[joint_names[i]] = std::max(std::fabs(qddot_limits.first[id]), std::fabs(qddot_limits.second[id])); 35 | } 36 | nh.setParam("qddot_max", map_acc_lims); 37 | 38 | ROS_INFO("Added ros parameter qddot_max"); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /examples/plugin/min_effort/src/MinEffort.cpp: -------------------------------------------------------------------------------- 1 | #include "MinEffort.h" 2 | 3 | #include "fmt/format.h" 4 | 5 | using namespace XBot::Cartesian; 6 | 7 | 8 | MinimumEffortImpl::MinimumEffortImpl(YAML::Node task_node, 9 | Context::ConstPtr context): 10 | TaskDescriptionImpl(task_node, 11 | context, 12 | "MinimumEffort", 13 | context->model()->getNv()), 14 | _step_size(1e-3) 15 | { 16 | /* Here you can parse custom YAML fields from task_node */ 17 | 18 | if(auto n = task_node["step_size"]) 19 | { 20 | _step_size = n.as(); 21 | } 22 | } 23 | 24 | double MinimumEffortImpl::getStepSize() const 25 | { 26 | return _step_size; 27 | } 28 | 29 | void XBot::Cartesian::MinimumEffortImpl::update(double time, double period) 30 | { 31 | // call base class 32 | TaskDescriptionImpl::update(time, period); 33 | 34 | // any custom update action that the task may need 35 | } 36 | 37 | void XBot::Cartesian::MinimumEffortImpl::reset() 38 | { 39 | // call base class 40 | TaskDescriptionImpl::reset(); 41 | 42 | // any custom reset action that the task may need 43 | } 44 | 45 | void XBot::Cartesian::MinimumEffortImpl::log(MatLogger2::Ptr logger, 46 | bool init_logger, 47 | int buf_size) 48 | { 49 | // call base class 50 | TaskDescriptionImpl::log(logger, init_logger, buf_size); 51 | 52 | if(init_logger) 53 | { 54 | // call logger->create 55 | return; 56 | } 57 | 58 | // call logger->add 59 | } 60 | 61 | 62 | CARTESIO_REGISTER_TASK_PLUGIN(MinimumEffortImpl, MinimumEffort) 63 | -------------------------------------------------------------------------------- /src/opensot/OpenSotImpl.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_OPENSOT_IMPL_H__ 2 | #define __XBOT_CARTESIAN_OPENSOT_IMPL_H__ 3 | 4 | 5 | #include 6 | #include 7 | #include "opensot/OpenSotTask.h" 8 | 9 | #include 10 | #include 11 | 12 | 13 | namespace XBot { namespace Cartesian { 14 | 15 | class OpenSotImpl : public CartesianInterfaceImpl 16 | { 17 | 18 | public: 19 | 20 | OpenSotImpl(ProblemDescription ik_problem, 21 | Context::Ptr context); 22 | 23 | virtual bool update(double time, double period) override; 24 | 25 | virtual ~OpenSotImpl() override; 26 | 27 | protected: 28 | 29 | 30 | private: 31 | 32 | typedef OpenSoT::tasks::Aggregated::TaskPtr TaskPtr; 33 | typedef OpenSoT::constraints::Aggregated::ConstraintPtr ConstraintPtr; 34 | 35 | void make_task_adapter(TaskDescription::Ptr); 36 | void make_constraint_adapter(ConstraintDescription::Ptr constr_desc); 37 | TaskPtr aggregated_from_stack(AggregatedTask stack); 38 | 39 | Eigen::VectorXd _qref; 40 | Eigen::VectorXd _x, _q, _dq, _ddq, _tau; 41 | Eigen::MatrixXd _J; 42 | 43 | std::vector _task_adapters; 44 | std::vector _constr_adapters; 45 | OpenSoT::OptvarHelper _vars; 46 | std::map _vars_map; 47 | 48 | OpenSoT::Solver::SolverPtr _solver; 49 | OpenSoT::AutoStack::Ptr _autostack; 50 | Eigen::MatrixXd _js_inertia_inv; 51 | bool _force_space_references; 52 | 53 | XBot::MatLogger2::Ptr _logger; 54 | 55 | }; 56 | 57 | 58 | } } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/plugin/angular_mom/AngularMomentumRos.cpp: -------------------------------------------------------------------------------- 1 | #include "AngularMomentumRos.h" 2 | #include 3 | #include "fmt/format.h" 4 | 5 | using namespace XBot::Cartesian; 6 | using namespace XBot::Cartesian::ServerApi; 7 | 8 | AngularMomentumRos::AngularMomentumRos(TaskDescription::Ptr task, 9 | XBot::ModelInterface::ConstPtr model): 10 | TaskRos(task, model) 11 | { 12 | /* Type cast to the required type, and throw on failure */ 13 | _ci_angmom = std::dynamic_pointer_cast(task); 14 | if(!_ci_angmom) throw std::runtime_error("Provided task description " 15 | "does not have expected type 'AngularMomentum'"); 16 | 17 | /* Open topics */ 18 | _ref_sub = _ctx.nh().subscribe(task->getName() + "/reference", 1, 19 | &AngularMomentumRos::on_ref_recv, this); 20 | 21 | _cur_ref_pub = _ctx.nh().advertise(task->getName() + "/current_reference", 22 | 1); 23 | /* Register type name */ 24 | registerType("AngularMomentum"); 25 | } 26 | 27 | void AngularMomentumRos::run(ros::Time time) 28 | { 29 | geometry_msgs::Vector3Stamped msg; 30 | msg.header.stamp = time; 31 | 32 | tf::vectorEigenToMsg(_ci_angmom->getReference(), 33 | msg.vector); 34 | 35 | _cur_ref_pub.publish(msg); 36 | } 37 | 38 | void AngularMomentumRos::on_ref_recv(geometry_msgs::Vector3StampedConstPtr msg) 39 | { 40 | Eigen::Vector3d ref; 41 | tf::vectorMsgToEigen(msg->vector, ref); 42 | 43 | _ci_angmom->setReference(ref); 44 | } 45 | 46 | CARTESIO_REGISTER_ROS_API_PLUGIN(AngularMomentumRos) 47 | -------------------------------------------------------------------------------- /include/cartesian_interface/utils/TaskFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_UTILS_TASK_FACTORY_H__ 2 | #define __XBOT_CARTESIAN_UTILS_TASK_FACTORY_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace XBot { namespace Cartesian { 12 | 13 | class TaskFactory 14 | { 15 | 16 | public: 17 | 18 | TaskFactory(YAML::Node prob_desc, 19 | Context::ConstPtr context); 20 | 21 | TaskDescription::Ptr makeTask(std::string task_name); 22 | 23 | private: 24 | 25 | YAML::Node _prob_desc; 26 | Context::ConstPtr _context; 27 | std::map _subtask_map; 28 | }; 29 | 30 | std::shared_ptr MakeTaskDescription(YAML::Node prob_desc, 31 | std::string task_name, 32 | Context::ConstPtr context 33 | ); 34 | 35 | std::shared_ptr MakeConstraintDescription(YAML::Node constr_node, 36 | ModelInterface::ConstPtr model, 37 | Context::ConstPtr context 38 | ); 39 | 40 | struct BadTaskDescription : public std::runtime_error 41 | { 42 | BadTaskDescription(std::string what); 43 | }; 44 | 45 | struct TaskIsSubtask : public std::exception 46 | { 47 | std::string real_task_name; 48 | std::vector indices; 49 | std::vector remove_indices; 50 | }; 51 | 52 | } } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotSubtask.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotSubtask.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotSubtaskAdapter::OpenSotSubtaskAdapter(TaskDescription::Ptr task, 6 | Context::ConstPtr context): 7 | OpenSotTaskAdapter(task, context) 8 | { 9 | _ci_subtask = std::dynamic_pointer_cast(task); 10 | 11 | if(!_ci_subtask) throw std::runtime_error("Provided task description " 12 | "does not have expected type 'Subtask'"); 13 | 14 | try 15 | { 16 | _task_adapter = OpenSotTaskAdapter::MakeInstance(_ci_subtask->getTask(), 17 | context); 18 | } 19 | catch(...) 20 | { 21 | fprintf(stderr, "Unable to construct task from subtask \n"); 22 | throw; 23 | } 24 | 25 | } 26 | 27 | TaskPtr OpenSotSubtaskAdapter::constructTask() 28 | { 29 | std::list indices_list(_ci_subtask->getSubtaskIndices().begin(), 30 | _ci_subtask->getSubtaskIndices().end()); 31 | 32 | return SotUtils::make_shared(_task_adapter->getOpenSotTask(), 33 | indices_list); 34 | } 35 | 36 | bool OpenSotSubtaskAdapter::initialize(const OpenSoT::OptvarHelper& vars) 37 | { 38 | return _task_adapter->initialize(vars) && 39 | OpenSotTaskAdapter::initialize(vars); 40 | } 41 | 42 | void OpenSotSubtaskAdapter::update(double time, double period) 43 | { 44 | return _task_adapter->update(time, period); 45 | } 46 | 47 | 48 | OpenSoT::OptvarHelper::VariableVector OpenSotSubtaskAdapter::getRequiredVariables() const 49 | { 50 | return _task_adapter->getRequiredVariables(); 51 | } 52 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/src/AngularMomentumRos.cpp: -------------------------------------------------------------------------------- 1 | #include "AngularMomentumRos.h" 2 | #include 3 | #include "fmt/format.h" 4 | 5 | using namespace XBot::Cartesian; 6 | using namespace XBot::Cartesian::ServerApi; 7 | 8 | AngularMomentumRos::AngularMomentumRos(TaskDescription::Ptr task, 9 | RosContext::Ptr ros_context): 10 | TaskRos(task, ros_context) 11 | { 12 | /* Type cast to the required type, and throw on failure */ 13 | _ci_angmom = std::dynamic_pointer_cast(task); 14 | if(!_ci_angmom) throw std::runtime_error("Provided task description " 15 | "does not have expected type 'AngularMomentum'"); 16 | 17 | /* Open topics */ 18 | _ref_sub = _ctx->nh().subscribe(task->getName() + "/reference", 1, 19 | &AngularMomentumRos::on_ref_recv, this); 20 | 21 | _cur_ref_pub = _ctx->nh().advertise(task->getName() + "/current_reference", 22 | 1); 23 | /* Register type name */ 24 | registerType("AngularMomentum"); 25 | } 26 | 27 | void AngularMomentumRos::run(ros::Time time) 28 | { 29 | geometry_msgs::Vector3Stamped msg; 30 | msg.header.stamp = time; 31 | 32 | tf::vectorEigenToMsg(_ci_angmom->getReference(), 33 | msg.vector); 34 | 35 | _cur_ref_pub.publish(msg); 36 | 37 | TaskRos::run(time); 38 | } 39 | 40 | void AngularMomentumRos::on_ref_recv(geometry_msgs::Vector3StampedConstPtr msg) 41 | { 42 | Eigen::Vector3d ref; 43 | tf::vectorMsgToEigen(msg->vector, ref); 44 | 45 | _ci_angmom->setReference(ref); 46 | } 47 | 48 | CARTESIO_REGISTER_ROS_API_PLUGIN(AngularMomentumRos, AngularMomentum) 49 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotPostural.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotPostural.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | 6 | OpenSotPosturalAdapter::OpenSotPosturalAdapter(TaskDescription::Ptr task, 7 | Context::ConstPtr context): 8 | OpenSotTaskAdapter(task, context), 9 | _use_inertia_matrix(false) 10 | { 11 | _ci_postural = std::dynamic_pointer_cast(task); 12 | 13 | if(!_ci_postural) throw std::runtime_error("Provided task description " 14 | "does not have expected type 'PosturalTask'"); 15 | } 16 | 17 | TaskPtr OpenSotPosturalAdapter::constructTask() 18 | { 19 | _opensot_postural = SotUtils::make_shared(const_cast(*_model)); 20 | 21 | return _opensot_postural; 22 | } 23 | 24 | bool OpenSotPosturalAdapter::initialize(const OpenSoT::OptvarHelper& vars) 25 | { 26 | if(vars.getAllVariables().size() > 0) 27 | { 28 | throw BadVariables("[OpenSotPosturalAdapter] requires default variables definition"); 29 | } 30 | 31 | if(!OpenSotTaskAdapter::initialize(vars)) 32 | { 33 | return false; 34 | } 35 | 36 | _use_inertia_matrix = _ci_postural->useInertiaMatrixWeight(); 37 | 38 | return true; 39 | } 40 | 41 | void OpenSotPosturalAdapter::update(double time, double period) 42 | { 43 | OpenSotTaskAdapter::update(time, period); 44 | 45 | if(_use_inertia_matrix) 46 | { 47 | _model->computeInertiaMatrix(_inertia_matrix); 48 | _opensot_postural->setWeight(_inertia_matrix); 49 | } 50 | 51 | _ci_postural->getReferencePosture(_qref); 52 | _ci_postural->getReferenceVelocity(_qdotref); 53 | _qdotref *= _ctx->params()->getControlPeriod(); 54 | _opensot_postural->setReference(_qref, _qdotref); 55 | } 56 | -------------------------------------------------------------------------------- /src/plugin/angular_mom/AngularMomentum.h: -------------------------------------------------------------------------------- 1 | #ifndef ANGULARMOMENTUM_H 2 | #define ANGULARMOMENTUM_H 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief The AngularMomentum class models the abstract 10 | * interface for the task. 11 | * 12 | * NOTE: it is mandatory to inherit **virtually** from TaskDescription 13 | */ 14 | class AngularMomentum : public virtual TaskDescription 15 | { 16 | 17 | public: 18 | 19 | CARTESIO_DECLARE_SMART_PTR(AngularMomentum) 20 | 21 | /* Define the task API (pure virtual methods) */ 22 | virtual Eigen::Vector3d getReference() const = 0; 23 | virtual void setReference(const Eigen::Vector3d& lref) = 0; 24 | 25 | }; 26 | 27 | /** 28 | * @brief The AngularMomentumImpl class implements the abstract 29 | * interface 30 | */ 31 | class AngularMomentumImpl : public TaskDescriptionImpl, 32 | public virtual AngularMomentum 33 | { 34 | 35 | public: 36 | 37 | CARTESIO_DECLARE_SMART_PTR(AngularMomentumImpl) 38 | 39 | /* The task implementation constructor signature must be 40 | * as follows */ 41 | AngularMomentumImpl(YAML::Node task_node, 42 | ModelInterface::ConstPtr model); 43 | 44 | /* Implement the task API */ 45 | Eigen::Vector3d getReference() const override; 46 | void setReference(const Eigen::Vector3d& lref) override; 47 | 48 | /* Customize update, reset and log */ 49 | void update(double time, double period) override; 50 | void reset() override; 51 | void log(MatLogger2::Ptr logger, bool init_logger, int buf_size) override; 52 | 53 | private: 54 | 55 | static constexpr double REF_TTL = 1.0; 56 | 57 | Eigen::Vector3d _lref; 58 | double _ref_timeout; 59 | }; 60 | 61 | } } 62 | 63 | #endif // ANGULARMOMENTUM_H 64 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/src/AngularMomentum.h: -------------------------------------------------------------------------------- 1 | #ifndef ANGULARMOMENTUM_H 2 | #define ANGULARMOMENTUM_H 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief The AngularMomentum class models the abstract 10 | * interface for the task. 11 | * 12 | * NOTE: it is mandatory to inherit **virtually** from TaskDescription 13 | */ 14 | class AngularMomentum : public virtual TaskDescription 15 | { 16 | 17 | public: 18 | 19 | CARTESIO_DECLARE_SMART_PTR(AngularMomentum) 20 | 21 | /* Define the task API (pure virtual methods) */ 22 | virtual Eigen::Vector3d getReference() const = 0; 23 | virtual void setReference(const Eigen::Vector3d& lref) = 0; 24 | 25 | }; 26 | 27 | /** 28 | * @brief The AngularMomentumImpl class implements the abstract 29 | * interface 30 | */ 31 | class AngularMomentumImpl : public TaskDescriptionImpl, 32 | public virtual AngularMomentum 33 | { 34 | 35 | public: 36 | 37 | CARTESIO_DECLARE_SMART_PTR(AngularMomentumImpl) 38 | 39 | /* The task implementation constructor signature must be 40 | * as follows */ 41 | AngularMomentumImpl(YAML::Node task_node, 42 | Context::ConstPtr context); 43 | 44 | /* Implement the task API */ 45 | Eigen::Vector3d getReference() const override; 46 | void setReference(const Eigen::Vector3d& lref) override; 47 | 48 | /* Customize update, reset and log */ 49 | void update(double time, double period) override; 50 | void reset() override; 51 | void log(MatLogger2::Ptr logger, bool init_logger, int buf_size) override; 52 | 53 | private: 54 | 55 | static constexpr double REF_TTL = 1.0; 56 | 57 | Eigen::Vector3d _lref; 58 | double _ref_timeout; 59 | }; 60 | 61 | } } 62 | 63 | #endif // ANGULARMOMENTUM_H 64 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/rt/LockfreeBufferImpl.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCKFREE_BUFFER_IMPL_H 2 | #define LOCKFREE_BUFFER_IMPL_H 3 | 4 | #include 5 | #include "utils/spsc_queue_ci.hpp" 6 | #include "TaskRt.h" 7 | 8 | namespace XBot { namespace Cartesian { 9 | 10 | class LockfreeBufferImpl : public CartesianInterfaceImpl 11 | { 12 | 13 | typedef std::function CallbackType; 14 | 15 | public: 16 | 17 | typedef std::shared_ptr Ptr; 18 | 19 | LockfreeBufferImpl(CartesianInterfaceImpl * ci, Context::Ptr context); 20 | 21 | void callAvailable(CartesianInterface * ci); 22 | 23 | void pushState(CartesianInterface const * ci, ModelInterface const * model); 24 | 25 | void updateState(); 26 | 27 | virtual bool reset(double time); 28 | 29 | virtual bool resetWorld(const Eigen::Affine3d& w_T_new_world); 30 | 31 | private: 32 | 33 | static const int CALL_QUEUE_SIZE = 128; 34 | static const int DEFAULT_QUEUE_SIZE = 10; 35 | 36 | template 37 | using LockFreeQueue = boost::lockfree::spsc_queue>; 38 | 39 | LockFreeQueue _cb_queue; 40 | 41 | struct ModelState 42 | { 43 | Eigen::VectorXd q, v, tau; 44 | }; 45 | 46 | ModelState _state_tmp; 47 | LockFreeQueue _model_state_queue; 48 | 49 | ModelState _state_tmp_read; 50 | ModelInterface::Ptr _model; 51 | 52 | std::vector _tasks; 53 | 54 | }; 55 | 56 | } // Cartesian 57 | } // XBot 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /examples/plugin/manipulability/src/Manipulability.h: -------------------------------------------------------------------------------- 1 | #ifndef MANIPULABILITY_H 2 | #define MANIPULABILITY_H 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | /** 9 | * @brief The Manipulability class models the abstract 10 | * interface for the task. 11 | * 12 | * NOTE: it is mandatory to inherit **virtually** from TaskDescription 13 | */ 14 | class Manipulability : public virtual TaskDescription 15 | { 16 | 17 | public: 18 | 19 | CARTESIO_DECLARE_SMART_PTR(Manipulability) 20 | 21 | /* Define the task API (pure virtual methods) */ 22 | 23 | virtual double getStepSize() const = 0; 24 | virtual const std::string& getBaseLink() const = 0; 25 | virtual const std::string& getDistalLink() const = 0; 26 | 27 | }; 28 | 29 | /** 30 | * @brief The ManipulabilityImpl class implements the abstract 31 | * interface 32 | */ 33 | class ManipulabilityImpl : public TaskDescriptionImpl, 34 | public virtual Manipulability 35 | { 36 | 37 | public: 38 | 39 | CARTESIO_DECLARE_SMART_PTR(ManipulabilityImpl) 40 | 41 | /* The task implementation constructor signature must be 42 | * as follows */ 43 | ManipulabilityImpl(YAML::Node task_node, 44 | Context::ConstPtr context); 45 | 46 | /* Implement the task API */ 47 | 48 | double getStepSize() const override; 49 | const std::string& getBaseLink() const; 50 | const std::string& getDistalLink() const; 51 | 52 | /* Customize update, reset and log */ 53 | void update(double time, double period) override; 54 | void reset() override; 55 | void log(MatLogger2::Ptr logger, bool init_logger, int buf_size) override; 56 | 57 | private: 58 | 59 | double _step_size; 60 | std::string _base_link; 61 | std::string _distal_link; 62 | 63 | }; 64 | 65 | } } 66 | 67 | #endif // Manipulability_H 68 | -------------------------------------------------------------------------------- /src/plugin/angular_mom/AngularMomentum.cpp: -------------------------------------------------------------------------------- 1 | #include "AngularMomentum.h" 2 | 3 | #include "fmt/format.h" 4 | 5 | using namespace XBot::Cartesian; 6 | 7 | 8 | AngularMomentumImpl::AngularMomentumImpl(YAML::Node task_node, 9 | XBot::ModelInterface::ConstPtr model): 10 | TaskDescriptionImpl(task_node, 11 | model, 12 | "AngularMomentum", 13 | 3), 14 | _lref(0,0,0), 15 | _ref_timeout(-1) 16 | { 17 | /* Here you can parse custom YAML fields from task_node */ 18 | 19 | } 20 | 21 | Eigen::Vector3d AngularMomentumImpl::getReference() const 22 | { 23 | return _lref; 24 | } 25 | 26 | void AngularMomentumImpl::setReference(const Eigen::Vector3d& lref) 27 | { 28 | _lref = lref; 29 | _ref_timeout = getTime() + REF_TTL; 30 | } 31 | 32 | 33 | 34 | void XBot::Cartesian::AngularMomentumImpl::update(double time, double period) 35 | { 36 | // call base class 37 | TaskDescriptionImpl::update(time, period); 38 | 39 | // if the last reference has expired, the set it to zero 40 | if(time > _ref_timeout) _lref.setZero(); 41 | 42 | } 43 | 44 | void XBot::Cartesian::AngularMomentumImpl::reset() 45 | { 46 | // call base class 47 | TaskDescriptionImpl::reset(); 48 | 49 | _lref.setZero(); 50 | } 51 | 52 | void XBot::Cartesian::AngularMomentumImpl::log(MatLogger2::Ptr logger, 53 | bool init_logger, 54 | int buf_size) 55 | { 56 | // call base class 57 | TaskDescriptionImpl::log(logger, init_logger, buf_size); 58 | 59 | if(init_logger) 60 | { 61 | logger->createVectorVariable(getName() + "_ref", 3, 1, buf_size); 62 | return; 63 | } 64 | 65 | logger->add(getName() + "_ref", _lref); 66 | } 67 | 68 | 69 | CARTESIO_REGISTER_TASK_PLUGIN(AngularMomentumImpl) 70 | -------------------------------------------------------------------------------- /examples/plugin/angular_mom/src/AngularMomentum.cpp: -------------------------------------------------------------------------------- 1 | #include "AngularMomentum.h" 2 | 3 | #include "fmt/format.h" 4 | 5 | using namespace XBot::Cartesian; 6 | 7 | 8 | AngularMomentumImpl::AngularMomentumImpl(YAML::Node task_node, 9 | Context::ConstPtr context): 10 | TaskDescriptionImpl(task_node, 11 | context, 12 | "AngularMomentum", 13 | 3), 14 | _lref(0,0,0), 15 | _ref_timeout(-1) 16 | { 17 | /* Here you can parse custom YAML fields from task_node */ 18 | 19 | } 20 | 21 | 22 | Eigen::Vector3d AngularMomentumImpl::getReference() const 23 | { 24 | return _lref; 25 | } 26 | 27 | void AngularMomentumImpl::setReference(const Eigen::Vector3d& lref) 28 | { 29 | _lref = lref; 30 | _ref_timeout = getTime() + REF_TTL; 31 | } 32 | 33 | 34 | 35 | void XBot::Cartesian::AngularMomentumImpl::update(double time, double period) 36 | { 37 | // call base class 38 | TaskDescriptionImpl::update(time, period); 39 | 40 | // if the last reference has expired, the set it to zero 41 | if(time > _ref_timeout) _lref.setZero(); 42 | 43 | } 44 | 45 | void XBot::Cartesian::AngularMomentumImpl::reset() 46 | { 47 | // call base class 48 | TaskDescriptionImpl::reset(); 49 | 50 | _lref.setZero(); 51 | } 52 | 53 | void XBot::Cartesian::AngularMomentumImpl::log(MatLogger2::Ptr logger, 54 | bool init_logger, 55 | int buf_size) 56 | { 57 | // call base class 58 | TaskDescriptionImpl::log(logger, init_logger, buf_size); 59 | 60 | if(init_logger) 61 | { 62 | logger->create(getName() + "_ref", 3, 1, buf_size); 63 | return; 64 | } 65 | 66 | logger->add(getName() + "_ref", _lref); 67 | } 68 | 69 | 70 | CARTESIO_REGISTER_TASK_PLUGIN(AngularMomentumImpl, AngularMomentum) 71 | -------------------------------------------------------------------------------- /src/ros/CartesianAnalyzer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | using namespace XBot::Cartesian; 7 | 8 | ManipulabilityAnalyzer * __g_manip; 9 | XBot::ModelInterface::Ptr _model; 10 | 11 | 12 | void solution_callback(const sensor_msgs::JointStateConstPtr& msg); 13 | 14 | int main(int argc, char **argv) 15 | { 16 | ros::init(argc, argv, "cartesian_analyzer"); 17 | ros::NodeHandle nh("cartesian"), nh_priv("~"); 18 | 19 | XBot::Logger::SetVerbosityLevel(XBot::Logger::Severity::HIGH); 20 | 21 | auto problem_yaml = Utils::LoadProblemDescription(); 22 | auto xbot_cfg = Utils::LoadOptionsFromParamServer(); 23 | 24 | _model = XBot::ModelInterface::getModel(xbot_cfg); 25 | 26 | auto ctx = std::make_shared( 27 | std::make_shared(0.01), 28 | _model); 29 | 30 | ProblemDescription ik_problem(problem_yaml, ctx); 31 | 32 | auto solution_sub = nh.subscribe("solution", 1, solution_callback); 33 | 34 | ManipulabilityAnalyzer manip(_model, ik_problem, "ci"); 35 | __g_manip = &manip; 36 | 37 | ros::spin(); 38 | 39 | return EXIT_SUCCESS; 40 | 41 | } 42 | 43 | 44 | void solution_callback(const sensor_msgs::JointStateConstPtr& msg) 45 | { 46 | /* Sync from current ik solution */ 47 | XBot::JointNameMap jnamemap; 48 | if(msg->name.size() != msg->position.size()) 49 | { 50 | throw std::runtime_error("msg->name.size() != msg->position.size()"); 51 | } 52 | 53 | for(int i = 0; i < msg->name.size(); i++) 54 | { 55 | jnamemap[msg->name.at(i)] = msg->position.at(i); 56 | } 57 | 58 | _model->setJointPosition(jnamemap); 59 | _model->update(); 60 | 61 | 62 | __g_manip->compute(); 63 | 64 | } 65 | 66 | -------------------------------------------------------------------------------- /examples/configs/coman_manipulability.yaml: -------------------------------------------------------------------------------- 1 | solver_options: 2 | regularization: 1e-3 3 | back_end: "qpoases" 4 | 5 | stack: 6 | - ["LeftFoot", "RightFoot", "ComXY"] 7 | - ["LeftArm", "RightArm"] 8 | - ["ManipulabilityLA", "ManipulabilityRA"] 9 | 10 | constraints: ["JointLimits", "VelocityLimits"] 11 | 12 | Base: 13 | type: Cartesian 14 | distal_link: base_link 15 | 16 | LeftFoot: 17 | type: "Cartesian" 18 | name: "left_foot" 19 | distal_link: "l_sole" 20 | 21 | RightFoot: 22 | type: "Cartesian" 23 | name: "right_foot" 24 | distal_link: "r_sole" 25 | 26 | LeftArm: 27 | type: "Cartesian" 28 | name: "left_hand" 29 | distal_link: "LSoftHand" 30 | lambda: 0.1 31 | 32 | RightArm: 33 | type: "Cartesian" 34 | name: "right_hand" 35 | distal_link: "RSoftHand" 36 | lambda: 0.1 37 | 38 | Postural: 39 | type: "Postural" 40 | lambda: 0.01 41 | weight: 42 | reference@v0: 0.0 43 | reference@v1: 0.0 44 | reference@v2: 0.0 45 | reference@v3: 0.0 46 | reference@v4: 0.0 47 | reference@v5: 0.0 48 | 49 | ComXY: 50 | type: "Com" 51 | lambda: 0.1 52 | indices: [0, 1] 53 | weight: 10 54 | 55 | VelocityLimits: 56 | type: "VelocityLimits" 57 | 58 | JointLimits: 59 | type: "JointLimits" 60 | 61 | ManipulabilityLA: 62 | type: Manipulability 63 | lib_name: libCartesioManipulabilityAddon.so 64 | step_size: 1e-2 65 | lambda: 1. 66 | base_link: torso 67 | distal_link: LSoftHand 68 | 69 | 70 | ManipulabilityRA: 71 | type: Manipulability 72 | lib_name: libCartesioManipulabilityAddon.so 73 | step_size: 1e-2 74 | lambda: 1. 75 | base_link: torso 76 | distal_link: RSoftHand 77 | 78 | ManipulabilityCOM: 79 | type: Manipulability 80 | lib_name: libCartesioManipulabilityAddon.so 81 | step_size: 1e-2 82 | lambda: 1. 83 | distal_link: com 84 | weight: 1000 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /bindings/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | #if(${CMAKE_VERSION} VERSION_LESS "3.12.0") 4 | # message("Please consider to switch to latest version to enable 5 | # more reliable Python3 binaries detection") 6 | #else() 7 | # find_package(Python${PYBIND11_PYTHON_VERSION} COMPONENTS Development Interpreter REQUIRED) 8 | #endif() 9 | 10 | set(PYBIND11_PYTHON_VERSION 3) 11 | find_package(pybind11 QUIET) 12 | 13 | message(STATUS "check for pybind11") 14 | if(${pybind11_FOUND}) 15 | 16 | message(STATUS "compiling python bindings -> ${PYTHON_EXECUTABLE}") 17 | 18 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 19 | pybind11_add_module(pyest pyForceEstimation.cpp) 20 | target_link_libraries(pyest PUBLIC CartesianInterface) 21 | 22 | install(TARGETS pyest 23 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) 24 | 25 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 26 | pybind11_add_module(pyci pyRosImpl.cpp) 27 | target_link_libraries(pyci PRIVATE CartesianInterface CartesioAngularMomentumAddon) 28 | 29 | install(TARGETS pyci 30 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) 31 | 32 | pybind11_add_module(roscpp_utils pyRosInit.cpp) 33 | target_link_libraries(roscpp_utils PRIVATE ${catkin_LIBRARIES}) 34 | 35 | install(TARGETS roscpp_utils 36 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) 37 | 38 | pybind11_add_module(impedance pyImpedance.cpp) 39 | target_link_libraries(impedance PRIVATE CartesianInterface) 40 | 41 | install(TARGETS impedance 42 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) 43 | 44 | install(FILES pyci_all.py 45 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) 46 | 47 | install(PROGRAMS interactive_client.py 48 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 49 | 50 | else() 51 | message(STATUS "pybind not found") 52 | endif() 53 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/OpenSotJointLimits.h: -------------------------------------------------------------------------------- 1 | #ifndef OPENSOTJOINTLIMITS_H 2 | #define OPENSOTJOINTLIMITS_H 3 | 4 | #include 5 | 6 | #include 7 | #include "OpenSotTask.h" 8 | 9 | #include 10 | #include 11 | 12 | using JointLimitsSoT = OpenSoT::constraints::velocity::JointLimits; 13 | using JointLimitsInvarianceSoT = OpenSoT::constraints::velocity::JointLimitsInvariance; 14 | 15 | namespace XBot { namespace Cartesian { 16 | class OpenSoTJointLimitsInvarianceAdapter : 17 | public OpenSotConstraintAdapter 18 | { 19 | public: 20 | OpenSoTJointLimitsInvarianceAdapter(ConstraintDescription::Ptr constr, Context::ConstPtr context); 21 | 22 | virtual ConstraintPtr constructConstraint() override; 23 | 24 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 25 | 26 | virtual void update(double time, double period) override; 27 | 28 | virtual ~OpenSoTJointLimitsInvarianceAdapter() override = default; 29 | private: 30 | JointLimitsInvarianceSoT::Ptr _opensot_jlim; 31 | JointLimitsInvariance::Ptr _ci_jlim; 32 | }; 33 | 34 | 35 | class OpenSotJointLimitsAdapter : 36 | public OpenSotConstraintAdapter 37 | { 38 | 39 | public: 40 | 41 | OpenSotJointLimitsAdapter(ConstraintDescription::Ptr constr, 42 | Context::ConstPtr context); 43 | 44 | virtual ConstraintPtr constructConstraint() override; 45 | 46 | virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; 47 | 48 | virtual void update(double time, double period) override; 49 | 50 | virtual ~OpenSotJointLimitsAdapter() override = default; 51 | 52 | protected: 53 | 54 | private: 55 | 56 | JointLimitsSoT::Ptr _opensot_jlim; 57 | JointLimits::Ptr _ci_jlim; 58 | }; 59 | 60 | 61 | 62 | 63 | } } 64 | 65 | #endif // OPENSOTJOINTLIMITS_H 66 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Postural.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_POSTURAL_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_POSTURAL_H__ 3 | 4 | #include 5 | 6 | namespace XBot { namespace Cartesian { 7 | 8 | 9 | /** 10 | * @brief Description of a postural (i.e. joint space) task 11 | * 12 | */ 13 | struct PosturalTask : virtual TaskDescription { 14 | 15 | typedef std::shared_ptr Ptr; 16 | typedef std::shared_ptr ConstPtr; 17 | 18 | virtual bool useInertiaMatrixWeight() const = 0; 19 | 20 | /** 21 | * @brief getReferencePosture 22 | * @param qref 23 | */ 24 | virtual void getReferencePosture(Eigen::VectorXd& qref) const = 0; 25 | 26 | /** 27 | * @brief getReferenceVelocity 28 | * @param qdotref 29 | */ 30 | virtual void getReferenceVelocity(Eigen::VectorXd& qdotref) const = 0; 31 | 32 | /** 33 | * @brief getReferencePosture 34 | * @param qref 35 | */ 36 | virtual void getReferencePosture(JointNameMap& qref) const = 0; 37 | 38 | /** 39 | * @brief setReferencePosture 40 | * @param qref 41 | */ 42 | virtual void setReferencePosture(const JointNameMap& qref) = 0; 43 | 44 | /** 45 | * @brief setReferencePosture 46 | * @param qref 47 | */ 48 | virtual void setReferencePosture(const Eigen::VectorXd& qref) = 0; 49 | 50 | /** 51 | * @brief setReferenceVelocity 52 | * @param qdotref 53 | */ 54 | virtual void setReferenceVelocity(const JointNameMap& qdotref) = 0; 55 | 56 | /** 57 | * @brief setReferenceVelocity 58 | * @param qdotref 59 | */ 60 | virtual void setReferenceVelocity(const Eigen::VectorXd& qdotref) = 0; 61 | 62 | }; 63 | 64 | } } 65 | 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /include/cartesian_interface/trajectory/Spline.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018 IIT-ADVR 3 | * Author: Arturo Laurenzi 4 | * email: arturo.laurenzi@iit.it 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this program. If not, see 18 | */ 19 | 20 | #ifndef __XBOT_CARTESIAN_TRAJ_SPLINE_H__ 21 | #define __XBOT_CARTESIAN_TRAJ_SPLINE_H__ 22 | 23 | #include 24 | 25 | namespace alglib 26 | { 27 | class spline1dinterpolant; 28 | } 29 | 30 | namespace XBot { namespace Cartesian { 31 | 32 | class Spline : public Trajectory 33 | { 34 | 35 | public: 36 | 37 | Spline(); 38 | 39 | virtual void compute(); 40 | 41 | virtual Eigen::Affine3d evaluate(double time, 42 | Eigen::Vector6d * const vel = nullptr, 43 | Eigen::Vector6d * const acc = nullptr); 44 | 45 | ~Spline(); 46 | 47 | private: 48 | 49 | void clear_all(); 50 | void add_knot(double t, const Eigen::Affine3d& T); 51 | 52 | std::vector _splines_pos, _splines_rot; 53 | std::vector> _values_pos, _values_rot; 54 | std::vector _knots; 55 | }; 56 | 57 | } } 58 | 59 | #endif 60 | 61 | 62 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/opensot/Plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef PLUGIN_H 2 | #define PLUGIN_H 3 | 4 | #define CARTESIO_REGISTER_OPENSOT_PLUGIN(FactoryName, ArgType, ClassName, RetType) \ 5 | extern "C" RetType* FactoryName(::XBot::Cartesian::ArgType::Ptr task, \ 6 | ::XBot::Cartesian::Context::ConstPtr context, \ 7 | ::XBot::Cartesian::detail::Version ci_ver) \ 8 | { \ 9 | ::XBot::Cartesian::detail::Version plugin_ver CARTESIO_ABI_VERSION; \ 10 | \ 11 | if(!plugin_ver.isCompatible(ci_ver)) \ 12 | { \ 13 | fprintf(stderr, \ 14 | "Incompatible plugin version %d.%d.%d (expected %d.%d.%d)", \ 15 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 16 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 17 | throw std::runtime_error("Version mismatch"); \ 18 | } \ 19 | \ 20 | if(!(ci_ver == plugin_ver)) \ 21 | { \ 22 | fprintf(stderr, \ 23 | "Different plugin version %d.%d.%d (expected %d.%d.%d)", \ 24 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 25 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 26 | } \ 27 | \ 28 | return new ClassName(task, context); \ 29 | } 30 | 31 | #define CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(ClassName, TaskType) \ 32 | CARTESIO_REGISTER_OPENSOT_PLUGIN(create_opensot_##TaskType##_adapter, \ 33 | TaskDescription, \ 34 | ClassName, \ 35 | ::XBot::Cartesian::OpenSotTaskAdapter) 36 | 37 | #define CARTESIO_REGISTER_OPENSOT_CONSTR_PLUGIN(ClassName, TaskType) \ 38 | CARTESIO_REGISTER_OPENSOT_PLUGIN(create_opensot_##TaskType##_adapter, \ 39 | ConstraintDescription, \ 40 | ClassName, \ 41 | ::XBot::Cartesian::OpenSotConstraintAdapter) 42 | 43 | #endif // PLUGIN_H 44 | -------------------------------------------------------------------------------- /docs/source/tasks/limits.rst: -------------------------------------------------------------------------------- 1 | JointLimits 2 | =========== 3 | A *JointLimits* constraint is specified by the ``type: JointLimits`` property. Its parameters 4 | are described below. 5 | 6 | limits 7 | ^^^^^^ 8 | map field, where the key represents a valid joint name, and the value is a ``[qmin, qmax]`` pair. 9 | The field is optional, and all missing joints take their limits from the URDF file. 10 | 11 | bound_scaling 12 | ^^^^^^^^^^^^^ 13 | Scaling factor for the joint range, w.r.t. the the center of the range. For example, with a 14 | ``bound_scaling: 2.0`` parameter, the range ``[0, 2]`` will be transformed into ``[-1, 3]``. 15 | The field is optional, and the default value is 1.0 16 | 17 | Example 18 | ------- 19 | 20 | .. code-block:: yaml 21 | 22 | MyJointLims: 23 | type: JointLimits 24 | bound_scaling: 0.9 # 90% of the specified range 25 | limits: 26 | j_arm1_1: [-1, 1] # custom value 27 | j_arm1_7: [0, 0] # lock joint to 0 28 | # all other joints take the URDF limits 29 | 30 | 31 | VelocityLimits 32 | ============== 33 | A *VelocityLimits* constraint is specified by the ``type: VelocityLimits`` property. Its parameters 34 | are described below. 35 | 36 | limits 37 | ^^^^^^ 38 | Either one of the following: 39 | 40 | - map field, where the key represents a valid joint name, and the value is a ``qdot_max`` positive float. 41 | - scalar field, which overwrites all limits from URDF to the given constant value 42 | The field is optional, and all missing joints take their limits from the URDF file. 43 | 44 | bound_scaling 45 | ^^^^^^^^^^^^^ 46 | Scaling factor for the joint velocity limit. 47 | The field is optional, and the default value is 1.0 48 | 49 | Example 50 | ------- 51 | 52 | .. code-block:: yaml 53 | 54 | MyVelLims: 55 | type: VelocityLimits 56 | bound_scaling: 0.9 # 90% of the velocity limit 57 | limits: 58 | j_arm1_1: 1 # custom limit 59 | # all other joints take the URDF limits 60 | 61 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_interface 4 | 0.0.0 5 | The cartesian_interface package 6 | 7 | 8 | 9 | 10 | Arturo Laurenzi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | tf 37 | 38 | catkin 39 | 40 | message_runtime 41 | tf 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/ros/server_api/PosturalRos.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/server_api/PosturalRos.h" 2 | 3 | using namespace XBot::Cartesian; 4 | using namespace XBot::Cartesian::ServerApi; 5 | 6 | PosturalRos::PosturalRos(PosturalTask::Ptr task, 7 | RosContext::Ptr context): 8 | TaskRos(task, context), 9 | _postural(task) 10 | { 11 | registerType("Postural"); 12 | 13 | _current_ref_pub = _ctx->nh().advertise(task->getName() + "/current_reference", 1); 14 | 15 | _ref_sub = _ctx->nh().subscribe(task->getName() + "/reference", 1, 16 | &PosturalRos::on_ref_recv, this); 17 | } 18 | 19 | void PosturalRos::run(ros::Time time) 20 | { 21 | TaskRos::run(time); 22 | 23 | sensor_msgs::JointState msg; 24 | 25 | if(_current_ref_pub.getNumSubscribers() == 0) 26 | { 27 | return; 28 | } 29 | 30 | // get posture reference (size = nq) 31 | _postural->getReferencePosture(_posture_ref); 32 | 33 | // get minimal representation of q 34 | // this has size = nv 35 | _model->positionToMinimal(_posture_ref, _posture_ref); 36 | 37 | // to deal with non-euclidean joints, we will publish the 38 | // minimal representation of q 39 | 40 | msg.header.stamp = time; 41 | msg.name.reserve(_model->getNv()); 42 | msg.position.reserve(_model->getNv()); 43 | int i = 0; 44 | for(const std::string& jname : _model->getVNames()) 45 | { 46 | msg.name.push_back(jname); 47 | msg.position.push_back(_posture_ref[i]); 48 | i++; 49 | } 50 | 51 | _current_ref_pub.publish(msg); 52 | } 53 | 54 | void PosturalRos::on_ref_recv(sensor_msgs::JointStateConstPtr msg) 55 | { 56 | JointNameMap posture_ref; 57 | 58 | for(int i = 0; i < msg->name.size(); i++) 59 | { 60 | if(msg->position.size() <= i) 61 | { 62 | continue; 63 | } 64 | 65 | posture_ref[msg->name[i]] = msg->position[i]; 66 | } 67 | 68 | _postural->setReferencePosture(posture_ref); 69 | } 70 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/rt/CInteractionRt.h: -------------------------------------------------------------------------------- 1 | #ifndef CINTERACTIONRT_H 2 | #define CINTERACTIONRT_H 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | 10 | class InteractionRt : public virtual InteractionTask, 11 | public CartesianRt 12 | { 13 | 14 | public: 15 | 16 | InteractionRt(InteractionTask::Ptr task); 17 | 18 | // TaskRt interface 19 | public: 20 | 21 | void callAvailable() override; 22 | void sendState(bool send) override; 23 | 24 | // CartesianTask interface 25 | public: 26 | 27 | CARTESIO_DECLARE_SMART_PTR(InteractionTask) 28 | 29 | const Impedance& getImpedance() override; 30 | 31 | const Eigen::Vector6d& getForceReference() const override; 32 | void getForceLimits(Eigen::Vector6d& fmin, 33 | Eigen::Vector6d& fmax) const override; 34 | 35 | bool setImpedance(const Impedance & impedance) override; 36 | 37 | void setForceReference(const Eigen::Vector6d& f) override; 38 | bool setForceLimits(const Eigen::Vector6d& fmin, 39 | const Eigen::Vector6d& fmax) override; 40 | 41 | void abortStiffnessTransition() override; 42 | bool setStiffnessTransition(const Interpolator::WayPointVector & way_points) override; 43 | State getStiffnessState() const override; 44 | 45 | private: 46 | 47 | typedef std::function CallbackType; 48 | LockFreeQueue _cb_queue; 49 | 50 | struct DataToClient 51 | { 52 | Impedance _impedance; 53 | 54 | Eigen::Vector6d _force ; 55 | Eigen::Vector6d _force_max; 56 | Eigen::Vector6d _force_min; 57 | 58 | State _stiffness_state; 59 | }; 60 | 61 | LockFreeQueue _to_cli_queue; 62 | DataToClient _cli_data, _rt_data; 63 | 64 | InteractionTask::Ptr _task_impl; 65 | 66 | public: 67 | void update(double time, double period) override; 68 | }; 69 | 70 | } } 71 | 72 | #endif // CINTERACTIONRT_H 73 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotConstraintFromTask.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotConstraintFromTask.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotConstraintFromTaskAdapter::OpenSotConstraintFromTaskAdapter(ConstraintDescription::Ptr constr, 6 | Context::ConstPtr context): 7 | OpenSotConstraintAdapter(constr, context) 8 | { 9 | auto constr_from_task = std::dynamic_pointer_cast(constr); 10 | 11 | if(!constr_from_task) 12 | { 13 | throw std::runtime_error("Provided task description " 14 | "does not have expected type 'ConstraintFromTask'"); 15 | } 16 | 17 | // construct and initialize task adapter 18 | try 19 | { 20 | _task_adapter = OpenSotTaskAdapter::MakeInstance(constr_from_task->getTask(), 21 | context); 22 | } 23 | catch(...) 24 | { 25 | fprintf(stderr, "Unable to construct task from constraint \n"); 26 | throw; 27 | } 28 | 29 | } 30 | 31 | ConstraintPtr OpenSotConstraintFromTaskAdapter::constructConstraint() 32 | { 33 | return SotUtils::make_shared(_task_adapter->getOpenSotTask()); 34 | } 35 | 36 | bool OpenSotConstraintFromTaskAdapter::initialize(const OpenSoT::OptvarHelper& vars) 37 | { 38 | // initialize underlying task and then base class 39 | return _task_adapter->initialize(vars) && 40 | OpenSotConstraintAdapter::initialize(vars); 41 | } 42 | 43 | void OpenSotConstraintFromTaskAdapter::update(double time, double period) 44 | { 45 | _task_adapter->update(time, period); 46 | } 47 | 48 | 49 | 50 | OpenSoT::OptvarHelper::VariableVector OpenSotConstraintFromTaskAdapter::getRequiredVariables() const 51 | { 52 | return _task_adapter->getRequiredVariables(); 53 | } 54 | 55 | 56 | void OpenSotConstraintFromTaskAdapter::processSolution(const Eigen::VectorXd &solution) 57 | { 58 | _task_adapter->processSolution(solution); 59 | } 60 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotCom.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotCom.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotComAdapter::OpenSotComAdapter(TaskDescription::Ptr task, 6 | Context::ConstPtr context): 7 | OpenSotTaskAdapter(task, context) 8 | { 9 | _ci_com = std::dynamic_pointer_cast(task); 10 | 11 | if(!_ci_com) throw std::runtime_error("Provided task description " 12 | "does not have expected type 'ComTask'"); 13 | } 14 | 15 | TaskPtr OpenSotComAdapter::constructTask() 16 | { 17 | _opensot_com = SotUtils::make_shared(const_cast(*_model)); 18 | 19 | return _opensot_com; 20 | } 21 | 22 | bool OpenSotComAdapter::initialize(const OpenSoT::OptvarHelper& vars) 23 | { 24 | if(vars.getAllVariables().size() > 0) 25 | { 26 | throw BadVariables("[OpenSotComAdapter] requires default variables definition"); 27 | } 28 | 29 | bool ret = OpenSotTaskAdapter::initialize(vars); 30 | if(!ret) return false; 31 | 32 | /* Register observer */ 33 | auto this_shared_ptr = std::dynamic_pointer_cast(shared_from_this()); 34 | _ci_com->registerObserver(this_shared_ptr); 35 | 36 | return true; 37 | } 38 | 39 | void OpenSotComAdapter::update(double time, double period) 40 | { 41 | // note: this will update lambda 42 | OpenSotTaskAdapter::update(time, period); 43 | 44 | // we implement velocity control by forcing lambda = 0.0 45 | auto ctrl = _ci_com->getControlMode(); 46 | 47 | if(ctrl == ControlType::Velocity) 48 | { 49 | _opensot_com->setLambda(0.0); 50 | } 51 | 52 | /* Update reference */ 53 | Eigen::Affine3d Tref; 54 | Eigen::Vector6d vref; 55 | _ci_com->getPoseReference(Tref, &vref); 56 | _opensot_com->setReference(Tref.translation(), vref.head<3>()*_ctx->params()->getControlPeriod()); 57 | } 58 | 59 | bool OpenSotComAdapter::onBaseLinkChanged() 60 | { 61 | return false; 62 | } 63 | 64 | bool OpenSotComAdapter::onControlModeChanged() 65 | { 66 | return true; 67 | } 68 | -------------------------------------------------------------------------------- /docs/source/progr_py.rst: -------------------------------------------------------------------------------- 1 | Programmatic API (Python) 2 | ========================= 3 | 4 | Basic usage 5 | ----------- 6 | .. code-block:: python 7 | 8 | from cartesian_interface.pyci_all import * 9 | cli = pyci.CartesianInterfaceRos() 10 | task = cli.getTask('MyTask') 11 | # use task... 12 | 13 | Check the :ref:`tutorial` for an extensive example of the Python API! 14 | The Python API closely matches the C++ API, for which you can check a reference by following 15 | :ref:`this link`. 16 | 17 | 18 | Python interactive session 19 | -------------------------- 20 | At any time, it is possible to run an interactive Python session which is 21 | completely pre-configured for CartesIO, by running ``rosrun cartesian_interface interactive_client.py``. 22 | Variables for all defined tasks will be dynamically created for the best convenience of use. 23 | 24 | Example: 25 | 26 | .. code-block:: bash 27 | 28 | $ rosrun cartesian_interface interactive_client.py 29 | 30 | Python 2.7.12 (default, Oct 8 2019, 14:14:10) 31 | Type "copyright", "credits" or "license" for more information. 32 | 33 | IPython 5.8.0 -- An enhanced Interactive Python. 34 | ? -> Introduction and overview of IPython's features. 35 | %quickref -> Quick reference. 36 | help -> Python's own help system. 37 | object? -> Details about 'object', use 'object??' for extra details. 38 | 39 | [success] Successfully added Cartesian task with 40 | BASE LINK: base_link 41 | DISTAL LINK: TCP 42 | [success] Successfully added postural task 'Postural' 43 | [success] Successfully added postural task 'MinVel' 44 | [success] Successfully added task 'JointLimits' with type 'Task' 45 | [success] Successfully added task 'VelocityLimits' with type 'Task' 46 | 47 | # TCP, Postural, etc... variables ready to use! 48 | In [1]: TCP.getPoseReference() 49 | Out[1]: 50 | (translation: [0.7581, 0, 0.1223] 51 | rotation : [ 0, 0.8415, 0, 0.5403], 52 | array([ 0., 0., 0., 0., 0., 0.]), 53 | array([ 0., 0., 0., 0., 0., 0.])) 54 | 55 | -------------------------------------------------------------------------------- /launch/cartesio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /include/cartesian_interface/problem/Interaction.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_INTERACTION_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_INTERACTION_H__ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace XBot { namespace Cartesian { 10 | 11 | 12 | class Impedance 13 | { 14 | public: 15 | 16 | Eigen::Matrix6d stiffness; 17 | Eigen::Matrix6d damping ; 18 | Eigen::Matrix6d mass ; 19 | 20 | Impedance(const Eigen::Matrix6d& stiffness = Eigen::Matrix6d::Zero(), 21 | const Eigen::Matrix6d& damping = Eigen::Matrix6d::Zero(), 22 | const Eigen::Matrix6d& mass = Eigen::Matrix6d::Identity()) 23 | : stiffness(stiffness), damping(damping), mass(mass) {} 24 | 25 | }; 26 | 27 | 28 | class InteractionTask : public virtual CartesianTask 29 | { 30 | 31 | public: 32 | 33 | CARTESIO_DECLARE_SMART_PTR(InteractionTask) 34 | 35 | virtual const Impedance & getImpedance () = 0; 36 | 37 | virtual const Eigen::Vector6d& getForceReference () const = 0; 38 | virtual void getForceLimits (Eigen::Vector6d& fmin, 39 | Eigen::Vector6d& fmax) const = 0; 40 | 41 | virtual bool setImpedance (const Impedance & impedance) = 0; 42 | 43 | 44 | virtual void setForceReference (const Eigen::Vector6d& f) = 0; 45 | virtual bool setForceLimits (const Eigen::Vector6d& fmin, 46 | const Eigen::Vector6d& fmax)= 0; 47 | 48 | virtual void abortStiffnessTransition () = 0; 49 | virtual bool setStiffnessTransition (const Interpolator::WayPointVector & way_points) = 0; 50 | virtual State getStiffnessState () const = 0; 51 | 52 | }; 53 | 54 | class AdmittanceTask : public virtual InteractionTask 55 | { 56 | 57 | public: 58 | 59 | CARTESIO_DECLARE_SMART_PTR(AdmittanceTask) 60 | 61 | virtual const Eigen::Vector6d& getForceDeadzone() const = 0; 62 | virtual const std::vector& getForceEstimationChains() const = 0; 63 | 64 | }; 65 | 66 | } } 67 | 68 | 69 | 70 | #endif // __XBOT_CARTESIAN_PROBLEM_INTERACTION_H__ 71 | -------------------------------------------------------------------------------- /cmake/GenerateDeb.cmake: -------------------------------------------------------------------------------- 1 | message("**** Creating target release_deb to generate .deb for ${PROJECT_NAME}, to use it type: make release_deb ****") 2 | include (InstallRequiredSystemLibraries) 3 | 4 | set(CPACK_PACKAGING_INSTALL_PREFIX "/opt/xbot" CACHE PATH "Deb package install prefix") 5 | set(CPACK_GENERATOR "DEB") 6 | set(CPACK_PACKAGE_NAME ${PROJECT_NAME}) 7 | 8 | set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE "amd64") 9 | set(CPACK_PACKAGE_ARCHITECTURE "amd64") 10 | 11 | set(CPACK_DEBIAN_PACKAGE_DEPENDS "open_sot , xbot_msgs , robotinterfaceros") 12 | 13 | #set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON) 14 | set(CPACK_DEBIAN_PACKAGE_GENERATE_SHLIBS ON) 15 | 16 | set(CPACK_DEBIAN_PACKAGE_MAINTAINER "arturo.laurenzi@iit.it") 17 | set(CPACK_DEBIAN_PACKAGE_DESCRIPTION "A ROS Based Real-Time Capable Cartesian 18 | Control Framework") 19 | 20 | find_program(GIT_SCM git DOC "Git version control") 21 | mark_as_advanced(GIT_SCM) 22 | find_file(GITDIR NAMES .git PATHS ${CMAKE_SOURCE_DIR} NO_DEFAULT_PATH) 23 | if (GIT_SCM AND GITDIR) 24 | execute_process( 25 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 26 | COMMAND ${GIT_SCM} log -1 "--pretty=format:%h" 27 | OUTPUT_VARIABLE GIT_SHA1_SHORT 28 | ) 29 | else() 30 | # No version control 31 | # e.g. when the software is built from a source tarball 32 | # and gitrevision.hh is packaged with it but no Git is available 33 | message(STATUS "Will not remake ${SRCDIR}/gitrevision.hh") 34 | endif() 35 | 36 | set(CPACK_PACKAGE_VERSION "${PROJECT_VERSION}-${GIT_SHA1_SHORT}") 37 | 38 | execute_process( 39 | COMMAND lsb_release -cs 40 | OUTPUT_VARIABLE LINUX_DISTRO_NAME 41 | ) 42 | 43 | string(REPLACE "\n" "" LINUX_DISTRO_NAME ${LINUX_DISTRO_NAME}) 44 | 45 | set(CPACK_PACKAGE_FILE_NAME ${CPACK_PACKAGE_NAME}-${CPACK_PACKAGE_VERSION}-${CPACK_PACKAGE_ARCHITECTURE}-${LINUX_DISTRO_NAME}) 46 | 47 | message(STATUS "Will generate package '${CPACK_PACKAGE_FILE_NAME}.deb'") 48 | 49 | include(CPack) 50 | 51 | add_custom_target(release_deb 52 | COMMAND "${CMAKE_CPACK_COMMAND}" 53 | COMMENT "Generating .deb using Cpack" 54 | DEPENDS ${PROJECT_NAME} 55 | ) 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /docs/source/porting.rst: -------------------------------------------------------------------------------- 1 | Porting code from v1.x 2 | ====================== 3 | Some of the differences w.r.t. version 1.x are summarized hereafter. 4 | 5 | General design 6 | -------------- 7 | 8 | - Almost all functionalities of CartesIO are now provided **task-wise**, rather than from 9 | a global ``CartesianInterface`` object as before. The old API has been preserved for 10 | backward compatibility. A task object is obtained by name with the ``getTask()`` method, 11 | and is then dynamically casted with ``std::dynamic_pointer_cast``. 12 | - The most part of the implementation has been moved to task-specific classes implementing 13 | 14 | - reference management (e.g. time-out, filtering, trajectory generation, ...) 15 | - ROS API 16 | - translation into OpenSoT tasks/constraints 17 | - ... 18 | - A ``Subtask`` task type has been introduced to distribute parts of a task over different 19 | priorities 20 | - Richer API 21 | - The ``ControlMode`` property which could task values among ``Position``, ``Velocity``, ``Disabled`` 22 | is now splitted into 23 | 24 | - an ``ActivationState`` which is defined for all tasks, and can be either ``Enabled`` or 25 | ``Disabled`` 26 | - a ``ControlMode`` which is defined for Cartesian tasks only, and can be either ``Position`` or 27 | ``Velocity`` 28 | 29 | 30 | ROS 31 | --- 32 | 33 | - ``task_name/state`` topic has been renamed to ``task_name/current_reference`` 34 | - ``world_odom`` TF frame has been removed, and the standard ``world`` is used instead 35 | 36 | 37 | 38 | C++ 39 | --- 40 | 41 | - The ROS Client has been renamed, from ``RosImpl`` to ``RosClient``. The same applies to 42 | the header file name. 43 | - A new ``Context`` object has been introduced to gather together a struct of ``Parameters`` 44 | (among which the control rate), and the ``ModelInterface`` 45 | - Almost all constructors which would take a ``ModelInterface`` an input, now take a 46 | ``Context`` shared pointer 47 | - Implementations of ``CartesianInterfaceImpl`` are now created with the static factory 48 | ``::MakeInstance``, rather than manually looking for the shared object and invoking 49 | its ``extern "C"`` factory method by name 50 | 51 | 52 | Python 53 | ------ 54 | 55 | TBD 56 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotInteraction.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotInteraction.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | Utils::ForceEstimation::WeakPtr OpenSotInteractionAdapter::_fest_weak; 6 | 7 | OpenSotInteractionAdapter::OpenSotInteractionAdapter(TaskDescription::Ptr task, 8 | Context::ConstPtr context): 9 | OpenSotCartesianAdapter(task, context), 10 | _fest_upd(false) 11 | { 12 | _ci_adm = std::dynamic_pointer_cast(task); 13 | 14 | if(!_ci_adm) throw std::runtime_error("Provided task description " 15 | "does not have expected type 'AdmittanceTask'"); 16 | 17 | 18 | } 19 | 20 | TaskPtr OpenSotInteractionAdapter::constructTask() 21 | { 22 | Eigen::VectorXd q; 23 | _model->getJointPosition(q); 24 | 25 | XBot::ForceTorqueSensor::ConstPtr ft; 26 | 27 | try 28 | { 29 | ft = _model->getForceTorque().at(_ci_adm->getDistalLink()); 30 | } 31 | catch(std::out_of_range& e) 32 | { 33 | /* We create a force estimator just once */ 34 | _fest = _fest_weak.lock(); 35 | if(!_fest) 36 | { 37 | _fest = std::make_shared(_model); 38 | _fest_weak = _fest; 39 | _fest_upd = true; 40 | } 41 | 42 | ft = _fest->add_link(_ci_adm->getDistalLink(), 43 | {}, 44 | _ci_adm->getForceEstimationChains()); 45 | } 46 | 47 | 48 | _opensot_adm = SotUtils::make_shared 49 | ("adm_" + _ci_adm->getName(), 50 | q, 51 | const_cast(*_model), 52 | _ci_adm->getBaseLink(), 53 | ft 54 | ); 55 | 56 | _opensot_adm->setDeadZone(_ci_adm->getForceDeadzone()); 57 | 58 | _opensot_cart = _opensot_adm; 59 | 60 | return _opensot_adm; 61 | } 62 | 63 | bool OpenSotInteractionAdapter::initialize(const OpenSoT::OptvarHelper& vars) 64 | { 65 | return OpenSotCartesianAdapter::initialize(vars); 66 | } 67 | 68 | void OpenSotInteractionAdapter::update(double time, double period) 69 | { 70 | OpenSotCartesianAdapter::update(time, period); 71 | 72 | if(_fest_upd) 73 | { 74 | _fest->update(); 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/problem/Postural.h: -------------------------------------------------------------------------------- 1 | #ifndef __XBOT_CARTESIAN_PROBLEM_POSTURAL_IMPL_H__ 2 | #define __XBOT_CARTESIAN_PROBLEM_POSTURAL_IMPL_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace XBot { namespace Cartesian { 8 | 9 | 10 | /** 11 | * @brief Description of a postural (i.e. joint space) task 12 | * 13 | */ 14 | struct PosturalTaskImpl : public TaskDescriptionImpl, 15 | public virtual PosturalTask 16 | { 17 | 18 | CARTESIO_DECLARE_SMART_PTR(PosturalTaskImpl) 19 | 20 | 21 | /** 22 | * @brief Construct a postural task from the number of robot dofs (including 6 virtual joints) 23 | * 24 | * @param ndof The number of robot dofs (including 6 virtual joints) 25 | */ 26 | PosturalTaskImpl(Context::ConstPtr context, int ndof); 27 | 28 | PosturalTaskImpl(YAML::Node node, Context::ConstPtr contextl); 29 | 30 | /** 31 | * @brief use_inertia_matrix if true the postural task is weighted with the inertia matrix. 32 | * To use it set the tag: 33 | * use_inertia: true 34 | */ 35 | bool useInertiaMatrixWeight() const override; 36 | 37 | void getReferencePosture(Eigen::VectorXd& qref) const override; 38 | 39 | void getReferencePosture(JointNameMap& qref) const override; 40 | 41 | void getReferenceVelocity(Eigen::VectorXd& qdotref) const override; 42 | 43 | void setReferencePosture(const JointNameMap& qref) override; 44 | 45 | void setReferencePosture(const Eigen::VectorXd& qref) override; 46 | 47 | void setReferenceVelocity(const JointNameMap& qdotref) override; 48 | 49 | void setReferenceVelocity(const Eigen::VectorXd& qdotref) override; 50 | 51 | void update(double time, double period) override; 52 | 53 | void reset() override; 54 | 55 | void setIndices(const std::vector& value) override; 56 | 57 | void setDisabledJoints(const std::vector& value) override; 58 | 59 | private: 60 | 61 | Eigen::VectorXd _qref, _qdotref; 62 | bool _use_inertia_matrix; 63 | 64 | }; 65 | 66 | } } 67 | 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotGaze.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotGaze.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotGazeAdapter::OpenSotGazeAdapter(TaskDescription::Ptr task, 6 | Context::ConstPtr context): 7 | OpenSotTaskAdapter(task, context) 8 | { 9 | _ci_cart = std::dynamic_pointer_cast(task); 10 | 11 | if(!_ci_cart) throw std::runtime_error("Provided task description " 12 | "does not have expected type 'GazeTask'"); 13 | 14 | } 15 | 16 | TaskPtr OpenSotGazeAdapter::constructTask() 17 | { 18 | _opensot_cart = SotUtils::make_shared(_ci_cart->getName(), 19 | const_cast(*_model), 20 | _ci_cart->getBaseLink(), 21 | _ci_cart->getDistalLink()); 22 | 23 | return _opensot_cart; 24 | } 25 | 26 | bool OpenSotGazeAdapter::initialize(const OpenSoT::OptvarHelper& vars) 27 | { 28 | if(vars.getAllVariables().size() > 0) 29 | { 30 | throw BadVariables("[OpenSotGazeAdapter] requires default variables definition"); 31 | } 32 | 33 | bool ret = OpenSotTaskAdapter::initialize(vars); 34 | if(!ret) return false; 35 | 36 | 37 | /* Register observer */ 38 | auto this_shared_ptr = std::dynamic_pointer_cast(shared_from_this()); 39 | _ci_cart->registerObserver(this_shared_ptr); 40 | 41 | return true; 42 | } 43 | 44 | void OpenSotGazeAdapter::update(double time, double period) 45 | { 46 | // note: this will update lambda 47 | OpenSotTaskAdapter::update(time, period); 48 | 49 | // we implement velocity control by forcing lambda = 0.0 50 | auto ctrl = _ci_cart->getControlMode(); 51 | 52 | if(ctrl == ControlType::Velocity) 53 | { 54 | _opensot_cart->setLambda(0.0); 55 | } 56 | 57 | /* Update reference */ 58 | Eigen::Affine3d Tref; 59 | Eigen::Vector6d vref; 60 | _ci_cart->getPoseReference(Tref, &vref); 61 | _opensot_cart->setGaze(Tref); 62 | } 63 | 64 | bool OpenSotGazeAdapter::onBaseLinkChanged() 65 | { 66 | return _opensot_cart->setBaseLink(_ci_cart->getBaseLink()); 67 | } 68 | 69 | bool OpenSotGazeAdapter::onControlModeChanged() 70 | { 71 | return true; 72 | } 73 | -------------------------------------------------------------------------------- /cmake/ExportCartesianInterface.cmake: -------------------------------------------------------------------------------- 1 | function(EXPORT_CARTESIAN_INTERFACE) 2 | 3 | if(NOT DEFINED ${PROJECT_NAME}_INSTALL_INCLUDE_DIR) 4 | set(${PROJECT_NAME}_INSTALL_INCLUDE_DIR include) 5 | endif() 6 | 7 | # ...executables... 8 | if(NOT DEFINED ${PROJECT_NAME}_INSTALL_BIN_DIR) 9 | set(${PROJECT_NAME}_INSTALL_BIN_DIR bin) 10 | endif() 11 | 12 | # ...libraries... 13 | if(NOT DEFINED ${PROJECT_NAME}_INSTALL_LIBRARY_DIR) 14 | set(${PROJECT_NAME}_INSTALL_LIBRARY_DIR lib) 15 | endif() 16 | 17 | # .. and cmake files 18 | if(NOT DEFINED ${PROJECT_NAME}_INSTALL_CONFIG_DIR) 19 | set(${PROJECT_NAME}_INSTALL_CONFIG_DIR 20 | ${${PROJECT_NAME}_INSTALL_LIBRARY_DIR}/cmake/${PROJECT_NAME}) 21 | endif() 22 | 23 | # write the package version file 24 | include(CMakePackageConfigHelpers) 25 | set(config_version_file ${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake) 26 | write_basic_package_version_file( 27 | ${config_version_file} 28 | VERSION ${PROJECT_VERSION} 29 | COMPATIBILITY AnyNewerVersion 30 | ) 31 | 32 | # create export for build tree 33 | export(EXPORT ${PROJECT_NAME}Targets 34 | NAMESPACE ${PROJECT_NAME}:: 35 | FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake" 36 | ) 37 | 38 | # Configure 'xxxConfig.cmake' for a build tree 39 | set(build_config ${CMAKE_BINARY_DIR}/${PROJECT_NAME}Config.cmake) 40 | configure_package_config_file( 41 | cmake/${PROJECT_NAME}Config.cmake.in 42 | ${build_config} 43 | INSTALL_DESTINATION "${PROJECT_BINARY_DIR}" 44 | ) 45 | 46 | install( 47 | EXPORT ${PROJECT_NAME}Targets 48 | NAMESPACE ${PROJECT_NAME}:: 49 | FILE ${PROJECT_NAME}Targets.cmake 50 | DESTINATION ${${PROJECT_NAME}_INSTALL_CONFIG_DIR} 51 | ) 52 | 53 | set(install_config ${PROJECT_BINARY_DIR}/CMakeFiles/${PROJECT_NAME}Config.cmake) 54 | configure_package_config_file( 55 | cmake/${PROJECT_NAME}Config.cmake.in 56 | ${install_config} 57 | INSTALL_DESTINATION ${${PROJECT_NAME}_INSTALL_CONFIG_DIR} 58 | ) 59 | 60 | # Install config files 61 | install( 62 | FILES 63 | ${config_version_file} 64 | ${install_config} 65 | DESTINATION "${${PROJECT_NAME}_INSTALL_CONFIG_DIR}" 66 | ) 67 | 68 | endfunction() 69 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(ExternalProject) 2 | 3 | # compite GTest 4 | find_package(GTest) 5 | find_package(Threads) 6 | 7 | if(NOT GTEST_FOUND) 8 | set(gTestSource "/usr/src/gtest") 9 | ExternalProject_Add(GTest-ext SOURCE_DIR ${gTestSource} 10 | PREFIX "${CMAKE_CURRENT_BINARY_DIR}/external" 11 | INSTALL_COMMAND "") 12 | set(GTEST_LIB_DIRS "${CMAKE_CURRENT_BINARY_DIR}/external/src/GTest-ext-build/") 13 | set(GTEST_INCLUDE_DIRS ${gTestSource}) 14 | set(GTEST_BOTH_LIBRARIES gtest gtest_main) 15 | set(GTEST_DEPENDS GTest-ext) 16 | endif() 17 | 18 | add_definitions(-DCARTESIO_TEST_CONFIG_PATH="${CMAKE_CURRENT_SOURCE_DIR}/configs/") 19 | add_definitions(-DCARTESIO__TEST_BINARY_PATH="${CMAKE_BINARY_DIR}/devel/lib/cartesian_interface") 20 | 21 | set(TestLibs CartesianInterface ${GTEST_BOTH_LIBRARIES}) 22 | include_directories(${GTEST_INCLUDE_DIRS}) 23 | link_directories(${GTEST_LIB_DIRS}) 24 | 25 | add_executable(TestTask TestTask.cpp) 26 | target_link_libraries(TestTask ${TestLibs}) 27 | add_dependencies(TestTask ${GTEST_DEPENDS} CartesianInterface) 28 | add_test(NAME TestTask COMMAND TestTask) 29 | 30 | add_executable(TestCartesian TestCartesian.cpp) 31 | target_link_libraries(TestCartesian ${TestLibs}) 32 | add_dependencies(TestCartesian ${GTEST_DEPENDS} CartesianInterface) 33 | add_test(NAME TestCartesian COMMAND TestCartesian) 34 | 35 | add_executable(TestApi TestApi.cpp) 36 | target_link_libraries(TestApi ${TestLibs}) 37 | add_dependencies(TestApi ${GTEST_DEPENDS} CartesianInterface) 38 | add_test(NAME TestApi COMMAND TestApi) 39 | 40 | add_executable(TestOpensot TestOpensot.cpp) 41 | target_link_libraries(TestOpensot ${TestLibs} dl) 42 | add_dependencies(TestOpensot ${GTEST_DEPENDS} CartesianInterface) 43 | add_test(NAME TestOpensot COMMAND TestOpensot) 44 | 45 | add_executable(TestRosClient TestRosClient.cpp) 46 | target_link_libraries(TestRosClient ${TestLibs}) 47 | add_dependencies(TestRosClient ${GTEST_DEPENDS} CartesianInterface) 48 | add_test(NAME TestRosClient COMMAND TestRosClient) 49 | 50 | add_executable(TestRt TestRt.cpp) 51 | target_link_libraries(TestRt dl ${TestLibs}) 52 | add_dependencies(TestRt ${GTEST_DEPENDS} CartesianInterface) 53 | add_test(NAME TestRt COMMAND TestRt) 54 | 55 | add_executable(TestRos TestRos.cpp) 56 | target_link_libraries(TestRos ${TestLibs} dl) 57 | add_dependencies(TestRos ${GTEST_DEPENDS} CartesianInterface ) 58 | add_test(NAME TestRos COMMAND TestRos) 59 | 60 | -------------------------------------------------------------------------------- /src/ros/client_api/PosturalRos.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/client_api/PosturalRos.h" 2 | #include "fmt/format.h" 3 | 4 | using namespace XBot::Cartesian; 5 | using namespace XBot::Cartesian::ClientApi; 6 | 7 | 8 | PosturalRos::PosturalRos(std::string name, ros::NodeHandle nh): 9 | TaskRos(name, nh), 10 | _curr_ref_recv(false) 11 | { 12 | _ref_pub = _nh.advertise(name + "/reference", 5); 13 | 14 | _current_ref_sub = _nh.subscribe(name + "/current_reference", 1, 15 | &PosturalRos::on_current_ref_recv, this); 16 | } 17 | 18 | bool PosturalRos::validate() 19 | { 20 | return TaskRos::validate() && _curr_ref_recv; 21 | } 22 | 23 | bool PosturalRos::useInertiaMatrixWeight() const 24 | { 25 | throw std::runtime_error(fmt::format("Unsupported function '{}'", 26 | __PRETTY_FUNCTION__)); 27 | } 28 | 29 | void PosturalRos::getReferencePosture(Eigen::VectorXd & qref) const 30 | { 31 | throw std::runtime_error(fmt::format("Unsupported function '{}'", 32 | __PRETTY_FUNCTION__)); 33 | } 34 | 35 | void PosturalRos::getReferencePosture(XBot::JointNameMap & qref) const 36 | { 37 | qref = _current_ref; 38 | } 39 | 40 | void PosturalRos::setReferencePosture(const XBot::JointNameMap & qref) 41 | { 42 | sensor_msgs::JointState msg; 43 | msg.header.stamp = ros::Time::now(); 44 | msg.name.reserve(qref.size()); 45 | msg.position.reserve(qref.size()); 46 | 47 | for(const auto& pair : qref) 48 | { 49 | msg.name.push_back(pair.first); 50 | msg.position.push_back(pair.second); 51 | } 52 | 53 | _ref_pub.publish(msg); 54 | } 55 | 56 | void PosturalRos::setReferencePosture(const Eigen::VectorXd & qref) 57 | { 58 | ///TODO 59 | } 60 | 61 | void PosturalRos::on_current_ref_recv(sensor_msgs::JointStateConstPtr msg) 62 | { 63 | _curr_ref_recv = true; 64 | 65 | for(int i = 0; i < msg->name.size(); i++) 66 | { 67 | if(msg->position.size() <= i) 68 | { 69 | continue; 70 | } 71 | 72 | _current_ref[msg->name[i]] = msg->position[i]; 73 | } 74 | 75 | } 76 | 77 | void PosturalRos::setReferenceVelocity(const JointNameMap& qdotref) 78 | { 79 | ///TODO 80 | } 81 | 82 | void PosturalRos::getReferenceVelocity(Eigen::VectorXd& qdotref) const 83 | { 84 | ///TODO 85 | } 86 | 87 | void PosturalRos::setReferenceVelocity(const Eigen::VectorXd& qdotref) 88 | { 89 | ///TODO 90 | } 91 | -------------------------------------------------------------------------------- /examples/plugin/manipulability/src/OpenSotManipulability.cpp: -------------------------------------------------------------------------------- 1 | #include "OpenSotManipulability.h" 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include "fmt/format.h" 8 | 9 | using namespace XBot::Cartesian; 10 | 11 | OpenSotManipulability::OpenSotManipulability(TaskDescription::Ptr task, 12 | Context::ConstPtr context): 13 | OpenSotTaskAdapter(task, context) 14 | { 15 | _ci_task = std::dynamic_pointer_cast(task); 16 | 17 | if(!_ci_task) throw std::runtime_error("Provided task description " 18 | "does not have expected type 'Manipulability'"); 19 | 20 | } 21 | 22 | TaskPtr OpenSotManipulability::constructTask() 23 | { 24 | std::string base_link = _ci_task->getBaseLink(); 25 | std::string distal_link = _ci_task->getDistalLink(); 26 | 27 | if(distal_link == "com") 28 | { 29 | _sot_task = std::make_shared(const_cast(*_model)); 30 | 31 | _sot_task_manip = SotUtils::make_shared(*_model, 32 | OpenSoT::tasks::velocity::CoM::asCoM(_sot_task), 33 | _ci_task->getStepSize()); 34 | } 35 | else 36 | { 37 | _sot_task = std::make_shared(distal_link, const_cast(*_model), distal_link, base_link); 38 | _sot_task_manip = SotUtils::make_shared(*_model, 39 | OpenSoT::tasks::velocity::Cartesian::asCartesian(_sot_task), 40 | _ci_task->getStepSize()); 41 | } 42 | 43 | 44 | return _sot_task_manip; 45 | } 46 | 47 | bool OpenSotManipulability::initialize(const ::OpenSoT::OptvarHelper& vars) 48 | { 49 | if(vars.getAllVariables().size() > 0) 50 | { 51 | throw BadVariables("[OpenSotManipulability] requires default variables definition"); 52 | } 53 | 54 | return OpenSotTaskAdapter::initialize(vars); 55 | } 56 | 57 | void OpenSotManipulability::update(double time, double period) 58 | { 59 | OpenSotTaskAdapter::update(time, period); 60 | 61 | // any custom update action 62 | } 63 | 64 | CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(OpenSotManipulability, Manipulability) 65 | 66 | -------------------------------------------------------------------------------- /src/problem/impl/Constraint.cpp: -------------------------------------------------------------------------------- 1 | #include "problem/Constraint.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | 6 | ConstraintFromTaskImpl::ConstraintFromTaskImpl(TaskDescriptionImpl::Ptr arg_task): 7 | TaskDescriptionImpl("ConstraintFromTask", 8 | arg_task->getName(), 9 | arg_task->getSize(), 10 | arg_task->getContext()), 11 | _task(arg_task) 12 | { 13 | 14 | } 15 | 16 | 17 | ConstraintDescription::Ptr XBot::Cartesian::MakeConstraintFromTask(XBot::Cartesian::TaskDescription::Ptr task) 18 | { 19 | if(auto task_impl = std::dynamic_pointer_cast(task)) 20 | { 21 | return std::make_shared(task_impl); 22 | } 23 | 24 | throw std::invalid_argument("MakeConstraintFromTask needs a TaskDescriptionImpl object as argument"); 25 | } 26 | 27 | 28 | TaskDescription::Ptr XBot::Cartesian::GetTaskFromConstraint(ConstraintDescription::Ptr constr) 29 | { 30 | auto constr_from_task = std::dynamic_pointer_cast(constr); 31 | 32 | if(constr_from_task) 33 | { 34 | return constr_from_task->getTask(); 35 | } 36 | else 37 | { 38 | return nullptr; 39 | } 40 | } 41 | 42 | 43 | bool XBot::Cartesian::ConstraintFromTaskImpl::validate() 44 | { 45 | return _task->validate(); 46 | } 47 | 48 | void XBot::Cartesian::ConstraintFromTaskImpl::update(double time, double period) 49 | { 50 | _task->update(time, period); 51 | } 52 | 53 | void XBot::Cartesian::ConstraintFromTaskImpl::reset() 54 | { 55 | _task->reset(); 56 | } 57 | 58 | void XBot::Cartesian::ConstraintFromTaskImpl::log(MatLogger2::Ptr logger, bool init_logger, int buf_size) 59 | { 60 | _task->log(logger, init_logger, buf_size); 61 | } 62 | 63 | 64 | bool ConstraintDescription::IsConstraint(TaskDescription::ConstPtr task) 65 | { 66 | return static_cast(AsConstraint(task)); 67 | } 68 | 69 | ConstraintDescription::Ptr ConstraintDescription::AsConstraint(TaskDescription::Ptr task) 70 | { 71 | return std::dynamic_pointer_cast(task); 72 | } 73 | 74 | ConstraintDescription::ConstPtr ConstraintDescription::AsConstraint(TaskDescription::ConstPtr task) 75 | { 76 | return std::dynamic_pointer_cast(task); 77 | } 78 | 79 | 80 | TaskDescription::Ptr XBot::Cartesian::ConstraintFromTaskImpl::getTask() 81 | { 82 | return _task; 83 | } 84 | 85 | 86 | bool XBot::Cartesian::ConstraintFromTaskImpl::getTaskError(Eigen::VectorXd &e) const 87 | { 88 | return _task->getTaskError(e); 89 | } 90 | -------------------------------------------------------------------------------- /include/cartesian_interface/utils/Manipulability.h: -------------------------------------------------------------------------------- 1 | #ifndef __CI_MANIPULABILITY_H__ 2 | #define __CI_MANIPULABILITY_H__ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace XBot { namespace Cartesian { 16 | 17 | class ManipulabilityAnalyzer 18 | { 19 | public: 20 | 21 | ManipulabilityAnalyzer(ModelInterface::ConstPtr model, 22 | ProblemDescription ik_problem, 23 | std::string tf_prefix 24 | ); 25 | 26 | ~ManipulabilityAnalyzer() 27 | { 28 | } 29 | 30 | void compute(); 31 | 32 | // static visualization_msgs::Marker ComputeEllipsoidFromJacobian(const Eigen::MatrixXd& J, 33 | // const Eigen::Vector3d& pos, 34 | // const std::string& base_link, 35 | // int start_idx); 36 | 37 | static visualization_msgs::Marker ComputeEllipsoidFromQuadraticForm(const Eigen::MatrixXd& JJtinv, 38 | const Eigen::Vector3d& pos, 39 | const std::string& base_link, 40 | int start_idx, 41 | const Eigen::Vector3d& rgb); 42 | 43 | private: 44 | 45 | bool compute_task_matrix(CartesianTask::Ptr task, Eigen::MatrixXd& A, Eigen::Affine3d& T); 46 | void publish(); 47 | 48 | XBot::MatLogger2::Ptr _logger; 49 | 50 | ModelInterface::ConstPtr _model; 51 | ProblemDescription _ik_problem; 52 | 53 | std::vector _nullspace_bases, _tasks; 54 | std::map _task_poses; 55 | std::map> _task_idx; 56 | 57 | std::map _pub_map_pos, _pub_map_rot; 58 | std::map _scale_pos, _scale_rot; 59 | ros::NodeHandle _nhpriv; 60 | 61 | std::string _tf_prefix_slash; 62 | 63 | 64 | }; 65 | 66 | 67 | } } 68 | 69 | 70 | 71 | 72 | 73 | 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /src/opensot/task_adapters/OpenSotCartesian.cpp: -------------------------------------------------------------------------------- 1 | #include "opensot/OpenSotCartesian.h" 2 | 3 | using namespace XBot::Cartesian; 4 | 5 | OpenSotCartesianAdapter::OpenSotCartesianAdapter(TaskDescription::Ptr task, 6 | Context::ConstPtr context): 7 | OpenSotTaskAdapter(task, context) 8 | { 9 | _ci_cart = std::dynamic_pointer_cast(task); 10 | 11 | if(!_ci_cart) throw std::runtime_error("Provided task description " 12 | "does not have expected type 'CartesianTask'"); 13 | 14 | } 15 | 16 | TaskPtr OpenSotCartesianAdapter::constructTask() 17 | { 18 | _opensot_cart = SotUtils::make_shared(_ci_cart->getName(), 19 | const_cast(*_model), 20 | _ci_cart->getDistalLink(), 21 | _ci_cart->getBaseLink()); 22 | 23 | return _opensot_cart; 24 | } 25 | 26 | bool OpenSotCartesianAdapter::initialize(const OpenSoT::OptvarHelper& vars) 27 | { 28 | if(vars.getAllVariables().size() > 0) 29 | { 30 | throw BadVariables("[OpenSotCartesianAdapter] requires default variables definition"); 31 | } 32 | 33 | bool ret = OpenSotTaskAdapter::initialize(vars); 34 | if(!ret) return false; 35 | 36 | 37 | /* Cartesian task specific parameters */ 38 | _opensot_cart->rotateToLocal(_ci_cart->isSubtaskLocal()); 39 | 40 | /* Register observer */ 41 | auto this_shared_ptr = std::dynamic_pointer_cast(shared_from_this()); 42 | _ci_cart->registerObserver(this_shared_ptr); 43 | 44 | return true; 45 | } 46 | 47 | void OpenSotCartesianAdapter::update(double time, double period) 48 | { 49 | // note: this will update lambda 50 | OpenSotTaskAdapter::update(time, period); 51 | 52 | // we implement velocity control by forcing lambda = 0.0 53 | auto ctrl = _ci_cart->getControlMode(); 54 | 55 | if(ctrl == ControlType::Velocity) 56 | { 57 | _opensot_cart->setLambda(0.0); 58 | } 59 | 60 | /* Update reference */ 61 | Eigen::Affine3d Tref; 62 | Eigen::Vector6d vref; 63 | _ci_cart->getPoseReference(Tref, &vref); 64 | _opensot_cart->setReference(Tref, vref*_ctx->params()->getControlPeriod()); 65 | } 66 | 67 | bool OpenSotCartesianAdapter::onBaseLinkChanged() 68 | { 69 | return _opensot_cart->setBaseLink(_ci_cart->getBaseLink()); 70 | } 71 | 72 | bool OpenSotCartesianAdapter::onControlModeChanged() 73 | { 74 | return true; 75 | } 76 | -------------------------------------------------------------------------------- /include/cartesian_interface/sdk/ros/Plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef PLUGIN_ROS_H 2 | #define PLUGIN_ROS_H 3 | 4 | #include 5 | 6 | #define CARTESIO_REGISTER_ROS_API_PLUGIN(ClassName, TaskType) \ 7 | extern "C" ::XBot::Cartesian::ServerApi::TaskRos * \ 8 | create_cartesio_##TaskType##_ros_api(::XBot::Cartesian::TaskDescription::Ptr task, \ 9 | ::XBot::Cartesian::RosContext::Ptr context, \ 10 | ::XBot::Cartesian::detail::Version ci_ver) \ 11 | { \ 12 | ::XBot::Cartesian::detail::Version plugin_ver CARTESIO_ABI_VERSION; \ 13 | \ 14 | if(!plugin_ver.isCompatible(ci_ver)) \ 15 | { \ 16 | fprintf(stderr, \ 17 | "Incompatible plugin version %d.%d.%d (expected %d.%d.%d)", \ 18 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 19 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 20 | throw std::runtime_error("Version mismatch"); \ 21 | } \ 22 | \ 23 | if(!(ci_ver == plugin_ver)) \ 24 | { \ 25 | fprintf(stderr, \ 26 | "Different plugin version %d.%d.%d (expected %d.%d.%d)", \ 27 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 28 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 29 | } \ 30 | \ 31 | return new ClassName(task, context); \ 32 | } 33 | 34 | 35 | #define CARTESIO_REGISTER_ROS_CLIENT_API_PLUGIN(ClassName, TaskType) \ 36 | extern "C" ::XBot::Cartesian::ClientApi::TaskRos * \ 37 | create_cartesio_##TaskType##_ros_client_api(::std::string name, \ 38 | ::ros::NodeHandle nh, \ 39 | ::XBot::Cartesian::detail::Version ci_ver) \ 40 | { \ 41 | ::XBot::Cartesian::detail::Version plugin_ver CARTESIO_ABI_VERSION; \ 42 | \ 43 | if(!plugin_ver.isCompatible(ci_ver)) \ 44 | { \ 45 | fprintf(stderr, \ 46 | "Incompatible plugin version %d.%d.%d (expected %d.%d.%d)", \ 47 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 48 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 49 | throw std::runtime_error("Version mismatch"); \ 50 | } \ 51 | \ 52 | if(!(ci_ver == plugin_ver)) \ 53 | { \ 54 | fprintf(stderr, \ 55 | "Different plugin version %d.%d.%d (expected %d.%d.%d)", \ 56 | plugin_ver.major, plugin_ver.minor, plugin_ver.patch, \ 57 | ci_ver.major, ci_ver.minor, ci_ver.patch); \ 58 | } \ 59 | \ 60 | return new ClassName(name, nh); \ 61 | } 62 | 63 | 64 | #endif // PLUGIN_H 65 | -------------------------------------------------------------------------------- /include/cartesian_interface/trajectory/Trajectory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018 IIT-ADVR 3 | * Author: Arturo Laurenzi 4 | * email: arturo.laurenzi@iit.it 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this program. If not, see 18 | */ 19 | 20 | #ifndef __XBOT_CARTESIAN_TRAJ_INTERPOLATION_H__ 21 | #define __XBOT_CARTESIAN_TRAJ_INTERPOLATION_H__ 22 | 23 | #include 24 | 25 | namespace XBot { namespace Cartesian { 26 | 27 | class Trajectory { 28 | 29 | public: 30 | 31 | typedef std::shared_ptr Ptr; 32 | typedef std::shared_ptr ConstPtr; 33 | 34 | struct WayPoint { 35 | 36 | Eigen::Affine3d frame; 37 | Eigen::Vector6d vel, acc; 38 | double time; 39 | 40 | WayPoint(); 41 | WayPoint(Eigen::Affine3d frame, double time); 42 | }; 43 | 44 | typedef std::vector WayPointVector; 45 | 46 | Trajectory(); 47 | 48 | void addWayPoint(double time, 49 | const Eigen::Affine3d& frame); 50 | 51 | void addWayPoint(const WayPoint& way_point, double time_offset = 0.0); 52 | 53 | void clear(); 54 | 55 | const std::vector& getWayPoints() const; 56 | 57 | virtual void compute(); 58 | 59 | virtual Eigen::Affine3d evaluate(double time, 60 | Eigen::Vector6d * const vel = nullptr, 61 | Eigen::Vector6d * const acc = nullptr); 62 | 63 | int getCurrentSegmentId(double time) const; 64 | 65 | bool isTrajectoryEnded(double time); 66 | 67 | 68 | protected: 69 | 70 | private: 71 | 72 | void sort_frames(); 73 | 74 | std::vector _frames; 75 | 76 | }; 77 | 78 | 79 | } } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /examples/plugin/manipulability/src/Manipulability.cpp: -------------------------------------------------------------------------------- 1 | #include "Manipulability.h" 2 | 3 | #include "fmt/format.h" 4 | 5 | using namespace XBot::Cartesian; 6 | 7 | std::string get_distal_link(YAML::Node task_node) 8 | { 9 | if(auto n = task_node["distal_link"]) 10 | { 11 | return n.as(); 12 | } 13 | else 14 | throw std::runtime_error("distal_link mandatory in Manipulability task!"); 15 | } 16 | 17 | 18 | ManipulabilityImpl::ManipulabilityImpl(YAML::Node task_node, 19 | Context::ConstPtr context): 20 | TaskDescriptionImpl(task_node, 21 | context, 22 | "Manipulability_" + get_distal_link(task_node), 23 | context->model()->getNv()), 24 | _step_size(1e-3) 25 | { 26 | /* Here you can parse custom YAML fields from task_node */ 27 | 28 | if(auto n = task_node["step_size"]) 29 | { 30 | _step_size = n.as(); 31 | } 32 | 33 | _distal_link = get_distal_link(task_node); 34 | if(_distal_link == "com") 35 | _base_link = "world"; 36 | else 37 | { 38 | if(auto n = task_node["base_link"]) 39 | { 40 | _base_link = n.as(); 41 | } 42 | else 43 | _base_link = "world"; 44 | } 45 | } 46 | 47 | const std::string& ManipulabilityImpl::getBaseLink() const 48 | { 49 | return _base_link; 50 | } 51 | 52 | const std::string& ManipulabilityImpl::getDistalLink() const 53 | { 54 | return _distal_link; 55 | } 56 | 57 | double ManipulabilityImpl::getStepSize() const 58 | { 59 | return _step_size; 60 | } 61 | 62 | void XBot::Cartesian::ManipulabilityImpl::update(double time, double period) 63 | { 64 | // call base class 65 | TaskDescriptionImpl::update(time, period); 66 | 67 | // any custom update action that the task may need 68 | } 69 | 70 | void XBot::Cartesian::ManipulabilityImpl::reset() 71 | { 72 | // call base class 73 | TaskDescriptionImpl::reset(); 74 | 75 | // any custom reset action that the task may need 76 | } 77 | 78 | void XBot::Cartesian::ManipulabilityImpl::log(MatLogger2::Ptr logger, 79 | bool init_logger, 80 | int buf_size) 81 | { 82 | // call base class 83 | TaskDescriptionImpl::log(logger, init_logger, buf_size); 84 | 85 | if(init_logger) 86 | { 87 | // call logger->create 88 | return; 89 | } 90 | 91 | // call logger->add 92 | } 93 | 94 | 95 | CARTESIO_REGISTER_TASK_PLUGIN(ManipulabilityImpl, Manipulability) 96 | --------------------------------------------------------------------------------