Save
This commit is contained in:
parent
c963bb9a3e
commit
d4d8aa507c
|
|
@ -0,0 +1,237 @@
|
|||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(poppy_ros)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
# add_compile_options(-std=c++11)
|
||||
|
||||
set (OpenCV_DIR "/usr/lib/opencv")
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
find_package(OpenCV REQUIRED COMPONENTS
|
||||
core
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a exec_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
|
||||
add_service_files(
|
||||
FILES
|
||||
IKService.srv
|
||||
)
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES poppy_ros
|
||||
# CATKIN_DEPENDS roscpp std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
/home/ros/SOFTWARE/toolkit-dynamixel/include
|
||||
/home/ros/SOFTWARE/toolkit-kinematics
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
/home/ros/SOFTWARE/eigen/Eigen
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/poppy_ros.cpp
|
||||
# )
|
||||
|
||||
#add_library(poppy_ros src/$DynamixelHandler.cpp)
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/poppy_ros_node.cpp)
|
||||
add_executable(poppy_ros src/poppy_ros.cpp src/toolkit-dynamixel/DynamixelHandler.cpp)
|
||||
|
||||
add_executable(ik_server src/ik_server.cpp src/toolkit-kinematics/Kinematics.cpp src/toolkit-kinematics/RotationMatrix.cpp src/toolkit-kinematics/TranslationMatrix.cpp src/toolkit-kinematics/TransformationMatrix.cpp)
|
||||
|
||||
add_executable(ik_client src/ik_client.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
target_link_libraries(poppy_ros dxl_x64_cpp rt ${catkin_LIBRARIES})
|
||||
|
||||
tsarget_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
|
||||
|
||||
target_link_libraries(ik_client ${catkin_LIBRARIES})
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# catkin_install_python(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
||||
# install(TARGETS ${PROJECT_NAME}_node
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
# install(TARGETS ${PROJECT_NAME}
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_poppy_ros.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
#add_executable(poppy_ros src/poppy_ros.cpp)
|
||||
#target_link_libraries(poppy_ros ${catkin_LIBRARIES})
|
||||
#add_executable(poppy_state src/poppy_state.cpp)
|
||||
#target_link_libraries(poppy_state ${catkin_LIBRARIES})
|
||||
set (OpenCV_DIR “/usr/lib/opencv”)
|
||||
find_package(OpenCV REQUIRED COMPONENTS core)
|
||||
include_directories(${OpenCV_INCLUDE_DIRS} /home/ros/SOFTWARE/eigen/Eigen)
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>poppy_ros</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The poppy_ros package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="ros@todo.todo">ros</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/poppy_ros</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<build_depend>message_generation</build_depend>
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "IKService.h"
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "ik_client");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::ServiceClient client = nh.serviceClient<poppy_ros::IKService>("ik_service");
|
||||
ros::Publisher joint_cmd_pub = nh.advertise<geometry_msgs::Twist>("/joint_cmd", 1);
|
||||
|
||||
poppy_ros::IKService srv;
|
||||
// Set Cartesian coordinates in the request
|
||||
// ...
|
||||
|
||||
if (client.call(srv)) {
|
||||
// Assuming joint values are to be published as a geometry_msgs::Twist message
|
||||
geometry_msgs::Twist joint_cmd;
|
||||
// Fill joint_cmd with received joint values
|
||||
// ...
|
||||
|
||||
joint_cmd_pub.publish(joint_cmd);
|
||||
ROS_INFO("Published joint command.");
|
||||
} else {
|
||||
ROS_ERROR("Failed to call service ik_service");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
#include <ros/ros.h>
|
||||
#include "Kinematics.h"
|
||||
#include "IKService.h"
|
||||
|
||||
Kinematics kinematics;
|
||||
|
||||
// Service callback function
|
||||
bool ikServiceHandler(poppy_ros::IKService::Request &req, poppy_ros::IKService::Response &res) {
|
||||
std::vector<double> cartesian_coords = {req.x, req.y, req.z};
|
||||
std::vector<double> joint_values;
|
||||
|
||||
// Perform inverse kinematics calculations
|
||||
bool success = kinematics.InverseKinematics(cartesian_coords, joint_values);
|
||||
|
||||
// Set the response
|
||||
if (success) {
|
||||
res.joint_values = joint_values;
|
||||
return true;
|
||||
} else {
|
||||
ROS_ERROR("Inverse kinematics calculation failed.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "ik_server");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// Load DH and IK parameters
|
||||
kinematics.loadDHParameters("/home/ros/catkin_ws/src/poppy_ros/src/poppy_dh.yaml");
|
||||
kinematics.loadIKParameters("/home/ros/catkin_ws/src/poppy_ros/src/poppy_ik.yaml");
|
||||
|
||||
// Initialize service server
|
||||
ros::ServiceServer service = nh.advertiseService("ik_service", ikServiceHandler);
|
||||
ROS_INFO("IK Service ready.");
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
dhThetaParams: [ 0., 0., 0., 0., 0., 0., 0. ]
|
||||
dhDParams: [ 2.5000000000000000e+00, 2.5000000000000000e+00, 0., 0., 0.,
|
||||
0., 0. ]
|
||||
dhAParams: [ 0., 0., 5.5000000000000000e+00, 3., 0., 5., 12. ]
|
||||
dhAlphaParams: [ 0., 90., 0., 0., -90., 90., 0. ]
|
||||
jointType: [ 0, 1, 1, 1, 0, 1, 1 ]
|
||||
nbJoints: 5
|
||||
jointOffset: [ 0., 0., 90., 0., -90., 0., 0. ]
|
||||
qiValues: [ 0., 0., 0., 0., 90. ]
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
maxNbIterations: 1000
|
||||
pController: 0.5
|
||||
rankThreshold: 0.2
|
||||
distanceThreshold: 0.01
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <vector>
|
||||
#include <cstdint> // For uint16_t
|
||||
|
||||
#include "DynamixelHandler.h"
|
||||
|
||||
std::string _poppyDxlPortName = "/dev/ttyUSB0";
|
||||
float _poppyDxlProtocol = 2.0;
|
||||
int _poppyDxlBaudRate = 1000000;
|
||||
|
||||
DynamixelHandler Dxl;
|
||||
|
||||
void jointCommandCallback(const geometry_msgs::Twist& joint_cmd)
|
||||
{
|
||||
// Create a vector of uint16_t
|
||||
std::vector<uint16_t> joint_values;
|
||||
|
||||
joint_values.push_back(joint_cmd.linear.x);
|
||||
joint_values.push_back(joint_cmd.linear.y);
|
||||
joint_values.push_back(joint_cmd.linear.z);
|
||||
|
||||
joint_values.push_back(joint_cmd.angular.x);
|
||||
joint_values.push_back(joint_cmd.angular.y);
|
||||
joint_values.push_back(joint_cmd.angular.z);
|
||||
|
||||
Dxl.sendTargetJointPosition(joint_values);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "poppy_ros");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::Subscriber sub = nh.subscribe("joint_cmd", 1, jointCommandCallback);
|
||||
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
|
||||
|
||||
ros::Rate loopRate(10);
|
||||
|
||||
// create a Twist message
|
||||
geometry_msgs::Twist jointPositionMsg;
|
||||
|
||||
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
||||
Dxl.setDeviceName(_poppyDxlPortName);
|
||||
Dxl.setProtocolVersion(_poppyDxlProtocol);
|
||||
Dxl.openPort();
|
||||
Dxl.setBaudRate(_poppyDxlBaudRate);
|
||||
Dxl.enableTorque(false);
|
||||
std::cout << std::endl;
|
||||
|
||||
|
||||
|
||||
ROS_INFO("===Launching Poppy node===");
|
||||
|
||||
// loop until Ctrl+C is pressed or ROS connectivity issues
|
||||
while(ros::ok())
|
||||
{
|
||||
//===RETRIEVE Dynamixel Motor positions====
|
||||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||||
bool bIsReadSuccessfull = Dxl.readCurrentJointPosition(l_vCurrentJointPosition);
|
||||
|
||||
// stores them into a msg
|
||||
if (bIsReadSuccessfull)
|
||||
{
|
||||
jointPositionMsg.linear.x = l_vCurrentJointPosition[0];
|
||||
jointPositionMsg.linear.y = l_vCurrentJointPosition[1];
|
||||
jointPositionMsg.linear.z = l_vCurrentJointPosition[2];
|
||||
jointPositionMsg.angular.x = l_vCurrentJointPosition[3];
|
||||
jointPositionMsg.angular.y = l_vCurrentJointPosition[4];
|
||||
jointPositionMsg.angular.z = l_vCurrentJointPosition[5];
|
||||
}
|
||||
|
||||
// publish the Twist message to the joint_position topic
|
||||
pub.publish(jointPositionMsg);
|
||||
|
||||
// spin once to let the process handle callback ad key stroke
|
||||
ros::spinOnce();
|
||||
|
||||
// sleep the right amout of time to comply with _fps
|
||||
loopRate.sleep();
|
||||
}
|
||||
|
||||
Dxl.enableTorque(false);
|
||||
Dxl.closePort();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
#include "DynamixelHandler.h"
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
|
||||
|
||||
// Global variables
|
||||
float _fps = 10.0f; // Hz
|
||||
|
||||
DynamixelHandler _oDxlHandler;
|
||||
ros::Publisher _jointPositionPublisher;
|
||||
std::string _poppyDxlPortName = "/dev/ttyUSB0";
|
||||
float _poppyDxlProtocol = 2.0;
|
||||
int _poppyDxlBaudRate = 1000000;
|
||||
int _nbJoints = 6;
|
||||
float _minJointCmd = 0;
|
||||
float _maxJointCmd = 1023;
|
||||
float _minJointAngle = -180.0f;
|
||||
float _maxJointAngle = 180.0f;
|
||||
|
||||
|
||||
|
||||
int convertAnglesToJointCmd(float fJointAngle)
|
||||
{
|
||||
// y = ax + b
|
||||
float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
|
||||
float b = _minJointCmd - a * _minJointAngle;
|
||||
float jointCmd = a * fJointAngle + b;
|
||||
return (int)jointCmd;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// create a node called poppy_ros
|
||||
ros::init(argc, argv, "poppy_ros", ros::init_options::NoSigintHandler);
|
||||
//ros::init(argc, argv, "autopilot");
|
||||
|
||||
// create a node handle
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// create a publisher to joint_position topic
|
||||
_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
|
||||
|
||||
// create a loop rate
|
||||
ros::Rate loopRate(_fps);
|
||||
|
||||
// create a Twist message
|
||||
geometry_msgs::Twist jointPositionMsg;
|
||||
|
||||
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
||||
_oDxlHandler.setDeviceName(_poppyDxlPortName);
|
||||
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
|
||||
_oDxlHandler.openPort();
|
||||
_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
|
||||
_oDxlHandler.enableTorque(false);
|
||||
std::cout << std::endl;
|
||||
|
||||
|
||||
|
||||
ROS_INFO("===Launching Poppy node===");
|
||||
|
||||
// loop until Ctrl+C is pressed or ROS connectivity issues
|
||||
while(ros::ok())
|
||||
{
|
||||
//===RETRIEVE Dynamixel Motor positions====
|
||||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||||
bool bIsReadSuccessfull = _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
|
||||
|
||||
// stores them into a msg
|
||||
if (bIsReadSuccessfull)
|
||||
{
|
||||
jointPositionMsg.linear.x = l_vCurrentJointPosition[0];
|
||||
jointPositionMsg.linear.y = l_vCurrentJointPosition[1];
|
||||
jointPositionMsg.linear.z = l_vCurrentJointPosition[2];
|
||||
jointPositionMsg.angular.x = l_vCurrentJointPosition[3];
|
||||
jointPositionMsg.angular.y = l_vCurrentJointPosition[4];
|
||||
jointPositionMsg.angular.z = l_vCurrentJointPosition[5];
|
||||
}
|
||||
|
||||
// publish the Twist message to the joint_position topic
|
||||
_jointPositionPublisher.publish(jointPositionMsg);
|
||||
|
||||
// spin once to let the process handle callback ad key stroke
|
||||
ros::spinOnce();
|
||||
|
||||
// sleep the right amout of time to comply with _fps
|
||||
loopRate.sleep();
|
||||
}
|
||||
|
||||
_oDxlHandler.enableTorque(false);
|
||||
_oDxlHandler.closePort();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,256 @@
|
|||
#include "DynamixelHandler.h"
|
||||
|
||||
DynamixelHandler::DynamixelHandler():
|
||||
m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0),
|
||||
m_pPacketHandler(nullptr), m_pPortHandler(nullptr),
|
||||
m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false),
|
||||
m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
DynamixelHandler::~DynamixelHandler()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool DynamixelHandler::openPort()
|
||||
{
|
||||
if (m_pPortHandler == nullptr)
|
||||
{
|
||||
std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl;
|
||||
m_bIsPortOpened = false;
|
||||
return m_bIsPortOpened;
|
||||
}
|
||||
|
||||
if (!m_bIsDeviceNameSet)
|
||||
{
|
||||
std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl;
|
||||
m_bIsPortOpened = false;
|
||||
return m_bIsPortOpened;
|
||||
}
|
||||
|
||||
if (m_bIsPortOpened)
|
||||
{
|
||||
std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl;
|
||||
return m_bIsPortOpened;
|
||||
}
|
||||
|
||||
if (m_pPortHandler->openPort())
|
||||
{
|
||||
std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl;
|
||||
m_bIsPortOpened = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl;
|
||||
m_bIsPortOpened = false;
|
||||
}
|
||||
return m_bIsPortOpened;
|
||||
}
|
||||
|
||||
void DynamixelHandler::closePort()
|
||||
{
|
||||
if (m_pPortHandler == nullptr)
|
||||
{
|
||||
std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl;
|
||||
m_bIsPortOpened = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!m_bIsPortOpened)
|
||||
{
|
||||
std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
m_pPortHandler->closePort();
|
||||
|
||||
std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl;
|
||||
m_bIsPortOpened = false;
|
||||
}
|
||||
|
||||
bool DynamixelHandler::setBaudRate(int i32BaudRate)
|
||||
{
|
||||
m_i32BaudRate = i32BaudRate;
|
||||
|
||||
if (nullptr != m_pPortHandler)
|
||||
{
|
||||
if (m_pPortHandler->setBaudRate(m_i32BaudRate))
|
||||
{
|
||||
std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl;
|
||||
m_bIsBaudRateSet = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl;
|
||||
m_bIsBaudRateSet = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl;
|
||||
m_bIsBaudRateSet = false;
|
||||
}
|
||||
return m_bIsBaudRateSet;
|
||||
}
|
||||
|
||||
void DynamixelHandler::setDeviceName(std::string sDeviceName)
|
||||
{
|
||||
m_sDeviceName = sDeviceName;
|
||||
m_bIsDeviceNameSet = true;
|
||||
|
||||
if (nullptr != m_pPortHandler)
|
||||
{
|
||||
delete m_pPortHandler;
|
||||
m_pPortHandler = nullptr;
|
||||
}
|
||||
|
||||
// Initialize PortHandler instance
|
||||
m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str());
|
||||
}
|
||||
|
||||
void DynamixelHandler::setProtocolVersion(float fProtocolVersion)
|
||||
{
|
||||
m_fProtocolVersion = fProtocolVersion;
|
||||
m_bIsProtocolVersionSet = true;
|
||||
|
||||
if (nullptr != m_pPacketHandler)
|
||||
{
|
||||
delete m_pPacketHandler;
|
||||
m_pPacketHandler = nullptr;
|
||||
}
|
||||
|
||||
m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion);
|
||||
}
|
||||
|
||||
bool DynamixelHandler::readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition)
|
||||
{
|
||||
bool bIsReadSuccessfull = false;
|
||||
|
||||
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
|
||||
{
|
||||
int dxl_comm_result = COMM_TX_FAIL; // Communication result
|
||||
uint8_t dxl_error = 0;
|
||||
uint16_t dxl_present_position = 0;
|
||||
|
||||
dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error);
|
||||
if (dxl_comm_result != COMM_SUCCESS)
|
||||
{
|
||||
std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
|
||||
bIsReadSuccessfull = false;
|
||||
}
|
||||
else if (dxl_error != 0)
|
||||
{
|
||||
std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
|
||||
bIsReadSuccessfull = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
vCurrentJointPosition.push_back(dxl_present_position);
|
||||
bIsReadSuccessfull = true;
|
||||
}
|
||||
}
|
||||
|
||||
return bIsReadSuccessfull;
|
||||
}
|
||||
|
||||
|
||||
bool DynamixelHandler::readCurrentJointTorque(std::vector<uint16_t>& vCurrentJointTorque)
|
||||
{
|
||||
bool bIsReadSuccessfull = false;
|
||||
|
||||
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
|
||||
{
|
||||
int dxl_comm_result = COMM_TX_FAIL; // Communication result
|
||||
uint8_t dxl_error = 0;
|
||||
uint16_t dxl_present_torque = 0;
|
||||
|
||||
dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_LOAD, &dxl_present_torque, &dxl_error);
|
||||
if (dxl_comm_result != COMM_SUCCESS)
|
||||
{
|
||||
std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
|
||||
bIsReadSuccessfull = false;
|
||||
}
|
||||
else if (dxl_error != 0)
|
||||
{
|
||||
std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
|
||||
bIsReadSuccessfull = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
vCurrentJointTorque.push_back(dxl_present_torque);
|
||||
bIsReadSuccessfull = true;
|
||||
}
|
||||
}
|
||||
|
||||
return bIsReadSuccessfull;
|
||||
}
|
||||
|
||||
|
||||
bool DynamixelHandler::sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition)
|
||||
{
|
||||
bool bIsSendSuccessfull = false;
|
||||
|
||||
// checks if the vector size is correct
|
||||
if (vTargetJointPosition.size() != NB_JOINTS)
|
||||
{
|
||||
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl;
|
||||
bIsSendSuccessfull = false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
|
||||
{
|
||||
int dxl_comm_result = COMM_TX_FAIL; // Communication result
|
||||
uint8_t dxl_error = 0;
|
||||
uint16_t dxl_present_position = 0;
|
||||
dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error);
|
||||
if (dxl_comm_result != COMM_SUCCESS)
|
||||
{
|
||||
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
|
||||
bIsSendSuccessfull = false;
|
||||
}
|
||||
else if (dxl_error != 0)
|
||||
{
|
||||
std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
|
||||
bIsSendSuccessfull = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
bIsSendSuccessfull = true;
|
||||
}
|
||||
}
|
||||
return bIsSendSuccessfull;
|
||||
|
||||
}
|
||||
|
||||
bool DynamixelHandler::enableTorque(bool bEnableTorque)
|
||||
{
|
||||
bool bIsSendSuccessfull = false;
|
||||
|
||||
for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
|
||||
{
|
||||
int dxl_comm_result = COMM_TX_FAIL; // Communication result
|
||||
uint8_t dxl_error = 0;
|
||||
|
||||
dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error);
|
||||
if (dxl_comm_result != COMM_SUCCESS)
|
||||
{
|
||||
std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
|
||||
bIsSendSuccessfull = false;
|
||||
}
|
||||
else if (dxl_error != 0)
|
||||
{
|
||||
std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
|
||||
bIsSendSuccessfull = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
bIsSendSuccessfull = true;
|
||||
}
|
||||
}
|
||||
return bIsSendSuccessfull;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,600 @@
|
|||
#include "Kinematics.h"
|
||||
|
||||
Kinematics::Kinematics():
|
||||
m_bAreDHParametersSet(false), m_bIsDatabaseLoaded(false),
|
||||
m_i32NbJoints(0),
|
||||
m_i32MaxNbIterations(0), m_f64PController(0.0), m_f64RankThreshold(0.0), m_f64DistanceThreshold(0.0)
|
||||
{}
|
||||
|
||||
Kinematics::Kinematics(std::vector<double>& vDHTheta, std::vector<double>& vDHD, std::vector<double>& vDHA, std::vector<double>& vDHAlpha, std::vector<int>& vJointType, std::vector<double>& vJointOffset, std::vector<double>& vQiValues, int i32NbJoints, int i32MaxNbIterations, double f64PController, double f64RankThreshold, double f64DistanceThreshold):
|
||||
m_bAreDHParametersSet(true), m_bIsDatabaseLoaded(false),
|
||||
m_vDHTheta(vDHTheta), m_vDHD(vDHD), m_vDHA(vDHA), m_vDHAlpha(vDHAlpha), m_vJointType(vJointType), m_vJointOffset(vJointOffset), m_vQiValues(vQiValues), m_vQiOriginalValues(vQiValues),
|
||||
m_i32NbJoints(i32NbJoints),
|
||||
m_i32MaxNbIterations(i32MaxNbIterations), m_f64PController(f64PController), m_f64RankThreshold(f64RankThreshold), m_f64DistanceThreshold(f64DistanceThreshold)
|
||||
{
|
||||
updateDHParameters();
|
||||
}
|
||||
|
||||
Kinematics::~Kinematics()
|
||||
{
|
||||
m_oRandomJointValues.release();
|
||||
m_oRandomEndEffectorPositions.release();
|
||||
}
|
||||
|
||||
void Kinematics::updateDHParameters(std::vector<double> vQiValues)
|
||||
{
|
||||
m_vQiValues = vQiValues;
|
||||
updateDHParameters();
|
||||
}
|
||||
|
||||
void Kinematics::updateDHParameters()
|
||||
{
|
||||
int l_i32JointIndex = 0;
|
||||
|
||||
for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
|
||||
{
|
||||
if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT)
|
||||
{
|
||||
/*std::cout << "l_i32JointIndex = " << l_i32JointIndex << std::endl;
|
||||
std::cout << "l_joint = " << l_joint << std::endl;
|
||||
std::cout << "m_vQiValues[l_i32JointIndex] = " << m_vQiValues[l_i32JointIndex] << std::endl;
|
||||
std::cout << "m_vJointOffset[l_joint]= " << m_vJointOffset[l_joint] << std::endl;
|
||||
*/
|
||||
m_vDHTheta[l_joint] = m_vQiValues[l_i32JointIndex] + m_vJointOffset[l_joint];
|
||||
l_i32JointIndex++;
|
||||
}
|
||||
else if (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
|
||||
{
|
||||
m_vDHD[l_joint] = m_vQiValues[l_i32JointIndex] + m_vJointOffset[l_joint];
|
||||
l_i32JointIndex++;
|
||||
}
|
||||
else //TODO find a way to avoid this else
|
||||
{
|
||||
m_vDHTheta[l_joint] = m_vJointOffset[l_joint];
|
||||
}
|
||||
}
|
||||
|
||||
/*for (int l_joint = 0; l_joint < m_vDHTheta.size(); l_joint ++)
|
||||
{
|
||||
std::cout << "(" << m_vDHTheta[l_joint] << ", " << m_vDHD[l_joint] << ", " << m_vDHA[l_joint] << ", " << m_vDHAlpha[l_joint] << ") " << std::endl;
|
||||
}*/
|
||||
|
||||
}
|
||||
|
||||
void Kinematics::setDHParameters(std::vector<double>& vDHTheta, std::vector<double>& vDHD, std::vector<double>& vDHA, std::vector<double>& vDHAlpha, std::vector<int>& vJointType, std::vector<double>& vJointOffset, std::vector<double>& vQiValues, int i32NbJoints)
|
||||
{
|
||||
m_vDHTheta = vDHTheta;
|
||||
m_vDHD = vDHD;
|
||||
m_vDHA = vDHA;
|
||||
m_vDHAlpha = vDHAlpha;
|
||||
m_vJointType = vJointType;
|
||||
m_vJointOffset = vJointOffset;
|
||||
m_vQiValues = vQiValues;
|
||||
m_i32NbJoints = i32NbJoints;
|
||||
|
||||
// check if the vectors have the same size
|
||||
if (m_vDHTheta.size() != m_vDHD.size() || m_vDHTheta.size() != m_vDHA.size() || m_vDHTheta.size() != m_vDHAlpha.size() || m_vDHTheta.size() != m_vJointType.size() || m_vDHTheta.size() != m_vJointOffset.size())
|
||||
{
|
||||
std::cerr << "(Kinematics::setDHParameters) The vectors of DH parameters do not have the same size!" << std::endl;
|
||||
m_bAreDHParametersSet = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
updateDHParameters();
|
||||
m_bAreDHParametersSet = true;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<double> Kinematics::getQiValues()
|
||||
{
|
||||
return m_vQiValues;
|
||||
}
|
||||
|
||||
cv::Mat Kinematics::dh2FK(int i32StartJointNumber)
|
||||
{
|
||||
cv::Mat l_oTransformationMatrix_jTee = cv::Mat::eye(4, 4, CV_64F);
|
||||
|
||||
if (!m_bAreDHParametersSet)
|
||||
{
|
||||
std::cerr << "(Kinematics::dh2FK) D-H parameters have not been set yet!" << std::endl;
|
||||
return l_oTransformationMatrix_jTee;
|
||||
}
|
||||
|
||||
if (i32StartJointNumber < 0 || i32StartJointNumber > m_vDHTheta.size())
|
||||
{
|
||||
std::cerr << "(Kinematics::dh2FK) the value of i32StartJointNumber is not in the right range!, i32StartJointNumber= "
|
||||
<< i32StartJointNumber << ", [0, " << m_vDHTheta.size() << "[" << std::endl;
|
||||
return l_oTransformationMatrix_jTee;
|
||||
}
|
||||
|
||||
|
||||
for (int l_joint = i32StartJointNumber; l_joint < m_vDHTheta.size(); l_joint++)
|
||||
{
|
||||
TransformationMatrix l_oThetaTransformationMatrix(0.0f, 0.0f, m_vDHTheta[l_joint], 1, 0.0f, 0.0f, 0.0f); //Rz
|
||||
TransformationMatrix l_oDTransformationMatrix(0.0f, 0.0f, 0.0f, 1, 0.0f, 0.0f, m_vDHD[l_joint]); //Tz
|
||||
TransformationMatrix l_oATransformationMatrix(0.0f, 0.0f, 0.0f, 1, m_vDHA[l_joint], 0.0f, 0.0f); //Tx
|
||||
TransformationMatrix l_oAlphaTransformationMatrix(m_vDHAlpha[l_joint], 0.0f, 0.0f, 1, 0.0f, 0.0f, 0.0f); //Rx
|
||||
|
||||
l_oTransformationMatrix_jTee *= l_oThetaTransformationMatrix.getTransformationMat() * l_oDTransformationMatrix.getTransformationMat() * l_oATransformationMatrix.getTransformationMat() * l_oAlphaTransformationMatrix.getTransformationMat();
|
||||
}
|
||||
|
||||
return l_oTransformationMatrix_jTee;
|
||||
}
|
||||
|
||||
|
||||
cv::Mat Kinematics::skewSymmetricMatrix(double fX, double fY, double fZ)
|
||||
{
|
||||
cv::Mat l_oSkewSymmetricMatrix(3, 3, CV_64F, 0.0f);
|
||||
|
||||
l_oSkewSymmetricMatrix.at<double>(0, 0) = 0.0f; l_oSkewSymmetricMatrix.at<double>(0, 1) = -fZ; l_oSkewSymmetricMatrix.at<double>(0, 2) = fY;
|
||||
l_oSkewSymmetricMatrix.at<double>(1, 0) = fZ; l_oSkewSymmetricMatrix.at<double>(1, 1) = 0.0f; l_oSkewSymmetricMatrix.at<double>(1, 2) = -fX;
|
||||
l_oSkewSymmetricMatrix.at<double>(2, 0) = -fY; l_oSkewSymmetricMatrix.at<double>(2, 1) = fX; l_oSkewSymmetricMatrix.at<double>(2, 2) = 0.0f;
|
||||
|
||||
return l_oSkewSymmetricMatrix;
|
||||
}
|
||||
|
||||
cv::Mat Kinematics::computeVee(cv::Mat oXtarget)
|
||||
{
|
||||
cv::Mat l_oVee(6, 1, CV_64F, 0.0f);
|
||||
|
||||
// estimates the current X (position and orientation of the end-effector)
|
||||
cv::Mat l_oXcurrent_hc = computeCurrentEndEffectorPosition();
|
||||
|
||||
cv::Mat l_oXcurrent(3, 1, CV_64F, 0.0f);
|
||||
l_oXcurrent_hc(cv::Rect(0, 0, l_oXcurrent_hc.cols, l_oXcurrent_hc.rows-1)).copyTo(l_oXcurrent); // remove homogeneous coordinate part i.e. last element (= 1)
|
||||
|
||||
// checks if the matrices have the same size
|
||||
if (l_oXcurrent.rows != oXtarget.rows || l_oXcurrent.cols != oXtarget.cols)
|
||||
{
|
||||
std::cerr << "(Kinematics::computeVee) Xcurrent and Xtarget matrices do not have the same size!" << std::endl;
|
||||
return l_oVee;
|
||||
}
|
||||
|
||||
// computes deltaX
|
||||
cv::Mat l_oDeltaX = oXtarget - l_oXcurrent;
|
||||
|
||||
// computes Xdot
|
||||
cv::Mat l_oXdot(6, 1, CV_64F, 0.0f); // adds zeros for orientation as we are controlling position only
|
||||
l_oDeltaX.copyTo(l_oXdot(cv::Rect(0, 0, l_oDeltaX.cols, l_oDeltaX.rows)));
|
||||
l_oXdot *= m_f64PController;
|
||||
|
||||
// retrieves eeRb from bTee
|
||||
cv::Mat l_oTransformationMatrix_bTee = dh2FK(0);
|
||||
TransformationMatrix l_oTransformationMatrix(l_oTransformationMatrix_bTee);
|
||||
cv::Mat l_oeeTb = l_oTransformationMatrix.getTransformationMatInverse();
|
||||
cv::Mat l_oeeRb = l_oeeTb(cv::Rect(0, 0, 3, 3));
|
||||
|
||||
// creates the transformation matrix Xdot->Vee i.e. [eeRb(3x3) zeros(3x3); zeros(3x3) eeRb(3x3)]
|
||||
cv::Mat l_oXdot2Vee(6, 6, CV_64F, 0.0f);
|
||||
l_oeeRb.copyTo(l_oXdot2Vee(cv::Rect(0, 0, l_oeeRb.cols, l_oeeRb.rows)));
|
||||
l_oeeRb.copyTo(l_oXdot2Vee(cv::Rect(3, 3, l_oeeRb.cols, l_oeeRb.rows)));
|
||||
|
||||
// computes Vee = Xdot2Vee * Xdot
|
||||
l_oVee = l_oXdot2Vee * l_oXdot;
|
||||
|
||||
return l_oVee;
|
||||
}
|
||||
|
||||
cv::Mat Kinematics::computeNumericalJacobian()
|
||||
{
|
||||
cv::Mat l_oNumericalJacobian(6, m_i32NbJoints, CV_64F, 0.0f);
|
||||
|
||||
int l_i32JointIndex = 0;
|
||||
for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
|
||||
{
|
||||
// checks if the current index corresponds to either a revolute or prismatic joint
|
||||
if ((m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT) || (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT))
|
||||
{
|
||||
// computes the transformation matrix jTee from the DH parameters
|
||||
cv::Mat l_oTransformationMatrix_jTee = dh2FK(l_joint);
|
||||
TransformationMatrix l_oTransformationMatrix(l_oTransformationMatrix_jTee);
|
||||
|
||||
// extracts the translation part and computes the skew symmetric matrix
|
||||
cv::Mat l_oTranslationMatrix_jtee = l_oTransformationMatrix.getTranslationMatrix()->getTranslationMat();
|
||||
cv::Mat l_oSkewSymmetricMatrix = skewSymmetricMatrix(l_oTranslationMatrix_jtee.at<double>(0, 0), l_oTranslationMatrix_jtee.at<double>(1, 0), l_oTranslationMatrix_jtee.at<double>(2, 0));
|
||||
|
||||
// extracts the rotation part eeRj
|
||||
cv::Mat l_oTransformationMatrix_eeTj = l_oTransformationMatrix.getTransformationMatInverse();
|
||||
cv::Mat l_oRotationMatrix_eeRj = l_oTransformationMatrix_eeTj(cv::Rect(0, 0, 3, 3));
|
||||
|
||||
// computes Tj i.e. [eeRj -eeRj*ssm; 0 eeRj]]
|
||||
cv::Mat l_oTj(6, 6, CV_64F, 0.0f);
|
||||
l_oRotationMatrix_eeRj.copyTo(l_oTj(cv::Rect(0, 0, l_oRotationMatrix_eeRj.cols, l_oRotationMatrix_eeRj.rows)));
|
||||
l_oRotationMatrix_eeRj.copyTo(l_oTj(cv::Rect(3, 3, l_oRotationMatrix_eeRj.cols, l_oRotationMatrix_eeRj.rows)));
|
||||
cv::Mat l_oRightUpperPartOfTj = -l_oRotationMatrix_eeRj * l_oSkewSymmetricMatrix;
|
||||
l_oRightUpperPartOfTj.copyTo(l_oTj(cv::Rect(3, 0, l_oRightUpperPartOfTj.cols, l_oRightUpperPartOfTj.rows)));
|
||||
|
||||
// selects the right column depending on the type of joint (revolute or prismatic)
|
||||
int l_i32IndexColumn = 0;
|
||||
if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT)
|
||||
l_i32IndexColumn = 5;
|
||||
else if (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
|
||||
l_i32IndexColumn = 2;
|
||||
|
||||
// fills the numerical Jacobian
|
||||
l_oTj(cv::Rect(l_i32IndexColumn, 0, 1, l_oTj.rows)).copyTo(l_oNumericalJacobian(cv::Rect(l_i32JointIndex, 0, 1, l_oTj.rows)));
|
||||
|
||||
// increments the joint index
|
||||
l_i32JointIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
return l_oNumericalJacobian;
|
||||
}
|
||||
|
||||
cv::Mat Kinematics::computePseudoInverseJacobian(cv::Mat oNumericalJacobian)
|
||||
{
|
||||
cv::Mat l_oPseudoInverseJacobian;
|
||||
if (false) // OpenCV SVD generates wrong pseudo inverse
|
||||
{
|
||||
// decomposes the numerical jacobian usind SVD
|
||||
cv::SVD l_oSVD;
|
||||
cv::Mat l_oW, l_oU, l_oVt;
|
||||
l_oSVD.compute(oNumericalJacobian, l_oW, l_oU, l_oVt, cv::SVD::FULL_UV);
|
||||
|
||||
// converts the singular values to matrix format (OpenCV stored them in a column vector)
|
||||
cv::Mat l_oSigma(6, m_i32NbJoints, CV_64F, 0.0);
|
||||
for (int l_joint = 0; l_joint < m_i32NbJoints; l_joint++)
|
||||
l_oSigma.at<double>(l_joint, l_joint) = l_oW.at<double>(l_joint, 0);
|
||||
|
||||
// computes the inverse of the sigma matrix (checks if the ratio between the current singular value and the first one is higher than a threshold)
|
||||
cv::Mat l_oSigmaInv(l_oSigma.cols, l_oSigma.rows, CV_64F, 0.0);
|
||||
|
||||
for (int l_singVal = 0; l_singVal < l_oSigma.cols; l_singVal++)
|
||||
{
|
||||
if (abs(l_oSigma.at<double>(l_singVal, l_singVal) / l_oSigma.at<double>(0, 0)) < m_f64RankThreshold)
|
||||
l_oSigmaInv.at<double>(l_singVal, l_singVal) = 0.0;
|
||||
else
|
||||
l_oSigmaInv.at<double>(l_singVal, l_singVal) = 1.0 / l_oSigma.at<double>(l_singVal, l_singVal);
|
||||
}
|
||||
|
||||
// computes the pseudoinverse matrix of the numerical jacobian
|
||||
l_oPseudoInverseJacobian = l_oVt * l_oSigmaInv * l_oU.t(); // = V * Sinv * Ut if U, S, V
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
// maps the OpenCV matrix with Eigen:
|
||||
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> oNumJacobian_Eigen(oNumericalJacobian.ptr<double>(), oNumericalJacobian.rows, oNumericalJacobian.cols);
|
||||
|
||||
// computes the Jacobi SVD of the Numerical Jacobian
|
||||
//Eigen::JacobiSVD<Eigen::MatrixXd> svd(oNumJacobian_Eigen, Eigen::ComputeFullV | Eigen:: ComputeFullU);
|
||||
Eigen::JacobiSVD<Eigen::MatrixXd> svd(oNumJacobian_Eigen, Eigen::ComputeFullV | Eigen:: ComputeFullU);
|
||||
svd.computeU();
|
||||
svd.computeV();
|
||||
|
||||
// stores the U, V, S matrices
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> oUMatrix_Eigen = svd.matrixU();
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> oVMatrix_Eigen = svd.matrixV();
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> oSigmaMatrix_Eigen = svd.singularValues();
|
||||
|
||||
// maps back to OpenCV matrices
|
||||
cv::Mat oUMatrix_OpenCV(oUMatrix_Eigen.rows(), oUMatrix_Eigen.cols(), CV_64F, oUMatrix_Eigen.data());
|
||||
cv::Mat oVMatrix_OpenCV(oVMatrix_Eigen.rows(), oVMatrix_Eigen.cols(), CV_64F, oVMatrix_Eigen.data());
|
||||
cv::Mat oSigmaMatrix_OpenCV(oSigmaMatrix_Eigen.rows(), oSigmaMatrix_Eigen.cols(), CV_64F, oSigmaMatrix_Eigen.data());
|
||||
|
||||
// computes the inverse of the sigma matrix (checks if the ratio between the current singular value and the first one is higher than a threshold)
|
||||
cv::Mat l_oSigmaInv(m_i32NbJoints, 6, CV_64F, 0.0);
|
||||
|
||||
for (int l_singVal = 0; l_singVal < oSigmaMatrix_OpenCV.rows; l_singVal++)
|
||||
{
|
||||
if (abs(oSigmaMatrix_OpenCV.at<double>(l_singVal, 0) / oSigmaMatrix_OpenCV.at<double>(0, 0)) < m_f64RankThreshold)
|
||||
l_oSigmaInv.at<double>(l_singVal, l_singVal) = 0.0;
|
||||
else
|
||||
l_oSigmaInv.at<double>(l_singVal, l_singVal) = 1.0 / oSigmaMatrix_OpenCV.at<double>(l_singVal, 0);
|
||||
}
|
||||
|
||||
// computes the pseudo inverse of the jacobian matrix
|
||||
l_oPseudoInverseJacobian = oVMatrix_OpenCV * l_oSigmaInv * oUMatrix_OpenCV.t(); // = V * Sinv * Ut if U, S, V
|
||||
}
|
||||
|
||||
return l_oPseudoInverseJacobian;
|
||||
}
|
||||
|
||||
|
||||
double Kinematics::computeDistanceToTarget(cv::Mat& oXtarget)
|
||||
{
|
||||
// estimates the current X (position and orientation of the end-effector)
|
||||
cv::Mat l_oXcurrent_hc = computeCurrentEndEffectorPosition();
|
||||
|
||||
// computes the Euclidian distance between the current point and the target one
|
||||
double l_fDistanceToTarget = 0.0f;
|
||||
for (int l_coord = 0; l_coord < 3; l_coord++)
|
||||
{
|
||||
l_fDistanceToTarget += pow(oXtarget.at<double>(l_coord, 0) - l_oXcurrent_hc.at<double>(l_coord, 0), 2);
|
||||
}
|
||||
|
||||
l_fDistanceToTarget = sqrt(l_fDistanceToTarget);
|
||||
|
||||
return l_fDistanceToTarget;
|
||||
}
|
||||
|
||||
void Kinematics::inverseKinematics(cv::Mat oXTarget)
|
||||
{
|
||||
bool l_bLoopInProgress = true;
|
||||
int l_i32NbIterations = 0;
|
||||
|
||||
if (m_bIsDatabaseLoaded)
|
||||
{
|
||||
findClosestCandidateInDatabase(oXTarget);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_vQiValues = m_vQiOriginalValues;
|
||||
}
|
||||
|
||||
while (l_bLoopInProgress)
|
||||
{
|
||||
cv::Mat l_oVee = computeVee(oXTarget);
|
||||
//std::cout << "inverseKinematics : l_oVee = " << l_oVee << std::endl;
|
||||
cv::Mat l_oJNum = computeNumericalJacobian();
|
||||
//std::cout << "inverseKinematics : l_oJNum = " << l_oJNum << std::endl;
|
||||
cv::Mat l_oJPseudoInv = computePseudoInverseJacobian(l_oJNum);
|
||||
//std::cout << "inverseKinematics : l_oJPseudoInv = " << l_oJPseudoInv << std::endl;
|
||||
cv::Mat deltaQ = l_oJPseudoInv * l_oVee;
|
||||
//std::cout << "inverseKinematics : deltaQ = " << deltaQ << std::endl;
|
||||
|
||||
int l_i32JointIndex = 0;
|
||||
for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
|
||||
{
|
||||
if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT || m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
|
||||
{
|
||||
m_vQiValues[l_i32JointIndex] += deltaQ.at<double>(l_i32JointIndex, 0) / m_f64PController;
|
||||
//std::cout << "(Kinematics::inverseKinematics) (joint index, joint value) = ( " << l_i32JointIndex << ", " << m_vQiValues[l_i32JointIndex] << ")" << std::endl;
|
||||
l_i32JointIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
updateDHParameters();
|
||||
|
||||
double l_fCurrentDistance = computeDistanceToTarget(oXTarget);
|
||||
std::cout << "---> #" << l_i32NbIterations << ", distance = " << l_fCurrentDistance << std::endl;
|
||||
|
||||
l_i32NbIterations++;
|
||||
|
||||
if (l_fCurrentDistance < m_f64DistanceThreshold || l_i32NbIterations > m_i32MaxNbIterations)
|
||||
l_bLoopInProgress = false;
|
||||
}
|
||||
}
|
||||
|
||||
cv::Mat Kinematics::computeCurrentEndEffectorPosition()
|
||||
{
|
||||
// estimates the current position of the end-effector)
|
||||
cv::Mat l_oTransformationMatrix_bTee = dh2FK(0);
|
||||
cv::Mat l_oXorigin_hc(4, 1, CV_64F, 0.0f); l_oXorigin_hc.at<double>(3, 0) = 1.0f; // homogeneous coordinates Xorigin = [0 0 0 1]'
|
||||
cv::Mat l_oXcurrent_hc = l_oTransformationMatrix_bTee * l_oXorigin_hc;
|
||||
|
||||
return l_oXcurrent_hc;
|
||||
}
|
||||
|
||||
void Kinematics::createKinematicDatabase(int i32NbSamples, std::string sKinematicDatabaseFilename, int i32MinValueRevoluteJoint, int i32MaxValueRevoluteJoint, int i32MinValuePrismaticJoint, int i32MaxValuePrismaticJoint)
|
||||
{
|
||||
// output matrices where the random qi values and the corresponding end-effector positions will be stored
|
||||
cv::Mat l_oRandomJointValues(i32NbSamples, m_i32NbJoints, CV_64F, 0.0f);
|
||||
cv::Mat l_oRandomEndEffectorPositions(i32NbSamples, 3, CV_64F, 0.0f);
|
||||
|
||||
// setups random number generators
|
||||
std::random_device l_oRandomDevice; // seed with a real random value, if available
|
||||
std::default_random_engine l_oRandomEngine(l_oRandomDevice());// create a random engine
|
||||
std::uniform_int_distribution<int> l_oUniformDistributionRevoluteJoint(i32MinValueRevoluteJoint, i32MaxValueRevoluteJoint);
|
||||
std::uniform_int_distribution<int> l_oUniformDistributionPrismaticJoint(i32MinValuePrismaticJoint, i32MaxValuePrismaticJoint);
|
||||
|
||||
// loops over the samples and generates random qi values and the corresponding end-effector positions
|
||||
for (int l_sample = 0; l_sample < i32NbSamples; l_sample++)
|
||||
{
|
||||
// generates random qi values depending on the joint type i.e. revolute or prismatic
|
||||
int l_i32JointIndex = 0;
|
||||
for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
|
||||
{
|
||||
if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT)
|
||||
{
|
||||
int l_i32RandomRevoluteJointValue = l_oUniformDistributionRevoluteJoint(l_oRandomEngine);
|
||||
l_oRandomJointValues.at<double>(l_sample, l_i32JointIndex) = (double)l_i32RandomRevoluteJointValue;
|
||||
m_vQiValues[l_i32JointIndex] = (double)l_i32RandomRevoluteJointValue;
|
||||
l_i32JointIndex++;
|
||||
|
||||
}
|
||||
else if (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
|
||||
{
|
||||
int l_i32RandomPrismaticJointValue = l_oUniformDistributionPrismaticJoint(l_oRandomEngine);
|
||||
l_oRandomJointValues.at<double>(l_sample, l_i32JointIndex) = (double)l_i32RandomPrismaticJointValue / 100.0f;
|
||||
m_vQiValues[l_i32JointIndex] = (double)l_i32RandomPrismaticJointValue / 100.0f;
|
||||
l_i32JointIndex++;
|
||||
}
|
||||
}
|
||||
updateDHParameters();
|
||||
|
||||
// computes the end-effector position
|
||||
cv::Mat l_oCurrentEndEffectorPosition = computeCurrentEndEffectorPosition();
|
||||
|
||||
// stores this data to a cv::Mat
|
||||
for (int l_coord= 0; l_coord < 3; l_coord++)
|
||||
l_oRandomEndEffectorPositions.at<double>(l_sample, l_coord) = l_oCurrentEndEffectorPosition.at<double>(l_coord, 0);
|
||||
|
||||
}
|
||||
|
||||
// creates a file storage to store the data
|
||||
cv::FileStorage l_fsKinematicDatabaseFile(sKinematicDatabaseFilename, cv::FileStorage::WRITE);
|
||||
|
||||
// checks if it was opened correctly
|
||||
if (l_fsKinematicDatabaseFile.isOpened() == false)
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::createKinematicDatabase) Could not open the kinematic database file for writing!";
|
||||
return;
|
||||
}
|
||||
|
||||
// writes data in the file
|
||||
l_fsKinematicDatabaseFile << "randomJointValues" << l_oRandomJointValues;
|
||||
l_fsKinematicDatabaseFile << "randomEndEffectorPositions" << l_oRandomEndEffectorPositions;
|
||||
|
||||
l_fsKinematicDatabaseFile.release();
|
||||
|
||||
}
|
||||
|
||||
void Kinematics::loadKinematicDatabase(std::string sKinematicDatabaseFilename)
|
||||
{
|
||||
// checks if the filename is empty
|
||||
if (sKinematicDatabaseFilename.empty())
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::loadKinematicDatabase) Kinematic database filename is empty!";
|
||||
return;
|
||||
}
|
||||
|
||||
// loads the databse file
|
||||
cv::FileStorage l_fsKinematicDatabaseFile(sKinematicDatabaseFilename, cv::FileStorage::READ);
|
||||
|
||||
// checks if file was successfully opened
|
||||
if (l_fsKinematicDatabaseFile.isOpened() == false)
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::loadKinematicDatabase) Could not open the kinematic database file for reading!";
|
||||
return;
|
||||
}
|
||||
|
||||
// reads the different variables
|
||||
cv::Mat l_oRandomJointValues;
|
||||
l_fsKinematicDatabaseFile["randomJointValues"] >> l_oRandomJointValues;
|
||||
if (l_oRandomJointValues.rows != 0 && l_oRandomJointValues.cols != 0)
|
||||
m_oRandomJointValues = l_oRandomJointValues;
|
||||
|
||||
cv::Mat l_oRandomEndEffectorPositions;
|
||||
l_fsKinematicDatabaseFile["randomEndEffectorPositions"] >> l_oRandomEndEffectorPositions;
|
||||
if (l_oRandomEndEffectorPositions.rows != 0 && l_oRandomEndEffectorPositions.cols != 0)
|
||||
m_oRandomEndEffectorPositions = l_oRandomEndEffectorPositions;
|
||||
|
||||
|
||||
|
||||
m_bIsDatabaseLoaded = true;
|
||||
}
|
||||
|
||||
void Kinematics::loadDHParameters(std::string sDHParametersFilename)
|
||||
{
|
||||
// checks if the filename is empty
|
||||
if (sDHParametersFilename.empty())
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::loadDHParameters) DH Parameters filename is empty!";
|
||||
return;
|
||||
}
|
||||
|
||||
// loads the databse file
|
||||
cv::FileStorage l_fsDHParametersFile(sDHParametersFilename, cv::FileStorage::READ);
|
||||
|
||||
// checks if file was successfully opened
|
||||
if (l_fsDHParametersFile.isOpened() == false)
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::loadDHParameters) Could not open the DH Parameters file for reading!";
|
||||
return;
|
||||
}
|
||||
|
||||
// reads the different variables
|
||||
std::vector<double> l_vDHTheta;
|
||||
l_fsDHParametersFile["dhThetaParams"] >> l_vDHTheta;
|
||||
if (l_vDHTheta.size() > 0)
|
||||
m_vDHTheta = l_vDHTheta;
|
||||
|
||||
std::vector<double> l_vDHD;
|
||||
l_fsDHParametersFile["dhDParams"] >> l_vDHD;
|
||||
if (l_vDHD.size() > 0)
|
||||
m_vDHD = l_vDHD;
|
||||
|
||||
std::vector<double> l_vDHA;
|
||||
l_fsDHParametersFile["dhAParams"] >> l_vDHA;
|
||||
if (l_vDHA.size() > 0)
|
||||
m_vDHA = l_vDHA;
|
||||
|
||||
std::vector<double> l_vDHAlpha;
|
||||
l_fsDHParametersFile["dhAlphaParams"] >> l_vDHAlpha;
|
||||
if (l_vDHAlpha.size() > 0)
|
||||
m_vDHAlpha = l_vDHAlpha;
|
||||
|
||||
std::vector<int> l_vJointType;
|
||||
l_fsDHParametersFile["jointType"] >> l_vJointType;
|
||||
if (l_vJointType.size() > 0)
|
||||
m_vJointType = l_vJointType;
|
||||
|
||||
l_fsDHParametersFile["nbJoints"] >> m_i32NbJoints;
|
||||
|
||||
std::vector<double> l_vJointOffset;
|
||||
l_fsDHParametersFile["jointOffset"] >> l_vJointOffset;
|
||||
if (l_vJointOffset.size() > 0)
|
||||
m_vJointOffset = l_vJointOffset;
|
||||
|
||||
std::vector<double> l_vQiValues;
|
||||
l_fsDHParametersFile["qiValues"] >> l_vQiValues;
|
||||
if (l_vQiValues.size() > 0)
|
||||
{
|
||||
m_vQiValues = l_vQiValues;
|
||||
m_vQiOriginalValues = l_vQiValues;
|
||||
}
|
||||
|
||||
updateDHParameters();
|
||||
|
||||
m_bAreDHParametersSet = true;
|
||||
}
|
||||
|
||||
|
||||
void Kinematics::loadIKParameters(std::string sIKParametersFilename)
|
||||
{
|
||||
// checks if the filename is empty
|
||||
if (sIKParametersFilename.empty())
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::loadIKParameters) IK Parameters filename is empty!";
|
||||
return;
|
||||
}
|
||||
|
||||
// loads the databse file
|
||||
cv::FileStorage l_fsIKParametersFile(sIKParametersFilename, cv::FileStorage::READ);
|
||||
|
||||
// checks if file was successfully opened
|
||||
if (l_fsIKParametersFile.isOpened() == false)
|
||||
{
|
||||
std::cerr << "[ERROR] (Kinematics::loadIKParameters) Could not open the IK Parameters file for reading!";
|
||||
return;
|
||||
}
|
||||
|
||||
// reads the different variables
|
||||
l_fsIKParametersFile["maxNbIterations"] >> m_i32MaxNbIterations;
|
||||
l_fsIKParametersFile["pController"] >> m_f64PController;
|
||||
l_fsIKParametersFile["rankThreshold"] >> m_f64RankThreshold;
|
||||
l_fsIKParametersFile["distanceThreshold"] >> m_f64DistanceThreshold;
|
||||
|
||||
m_bAreDHParametersSet = true;
|
||||
}
|
||||
|
||||
|
||||
void Kinematics::findClosestCandidateInDatabase(cv::Mat& oXtarget)
|
||||
{
|
||||
double l_fClosestSampleDistanceToTarget = std::numeric_limits<double>::max();
|
||||
int l_i32ClosestSampleIndex = 0;
|
||||
|
||||
//std::cout << "m_oRandomEndEffectorPositions.rows = " << m_oRandomEndEffectorPositions.rows << std::endl;
|
||||
//std::cout << "m_oRandomEndEffectorPositions.cols = " << m_oRandomEndEffectorPositions.cols << std::endl;
|
||||
// looks for the closest candidate in the database
|
||||
for (int l_sample = 0; l_sample < m_oRandomEndEffectorPositions.rows; l_sample++)
|
||||
{
|
||||
double l_fCurrentSampleDistanceToTarget = 0.0f;
|
||||
for (int l_coord = 0; l_coord < 3; l_coord++)
|
||||
l_fCurrentSampleDistanceToTarget += pow( oXtarget.at<double>(l_coord, 0) - m_oRandomEndEffectorPositions.at<double>(l_sample, l_coord), 2);
|
||||
|
||||
if (l_fCurrentSampleDistanceToTarget < l_fClosestSampleDistanceToTarget)
|
||||
{
|
||||
l_fClosestSampleDistanceToTarget = l_fCurrentSampleDistanceToTarget;
|
||||
l_i32ClosestSampleIndex = l_sample;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "l_fClosestSampleDistanceToTarget = " << l_fClosestSampleDistanceToTarget << std::endl;
|
||||
std::cout << "l_i32ClosestSampleIndex = " << l_i32ClosestSampleIndex << std::endl;
|
||||
|
||||
// change the DH parameters to become the closest candidate in the database
|
||||
int l_i32JointIndex = 0;
|
||||
for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
|
||||
{
|
||||
if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT || m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
|
||||
{
|
||||
m_vQiValues[l_joint] = m_oRandomJointValues.at<double>(l_i32ClosestSampleIndex, l_i32JointIndex);
|
||||
l_i32JointIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
updateDHParameters();
|
||||
}
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
float64 x
|
||||
float64 y
|
||||
float64 z
|
||||
---
|
||||
float64[] joint_values
|
||||
|
||||
Loading…
Reference in New Issue