Skip to content

Commit

Permalink
add namespace and config parsing
Browse files Browse the repository at this point in the history 
* use libconfig to parse config files
* move utilities unrelated to Kinova to separate source files
* add namespace kinova_ctrl to sources
  • Loading branch information
@minhnh
minhnh committed Jul 5, 2020
1 parent 931c224 commit 876d78a
Show file tree
Hide file tree
Showing 10 changed files with 227 additions and 107 deletions.
11 changes: 10 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,14 @@ project(KinovaControlExperiments VERSION 0.0.1 LANGUAGES CXX)
find_package(kdl_parser)
find_package(orocos_kdl)

# configuration library
find_library(CONFIG_CPP_LIBRARY "config++" "/usr/lib" "/usr/local/lib")
if (NOT CONFIG_CPP_LIBRARY)
MESSAGE(FATAL_ERROR "required config++ library not found, try installing libconfig++ and libconfig++-dev")
else()
MESSAGE("found config++ library at ${CONFIG_CPP_LIBRARY}")
endif()

set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_VERBOSE_MAKEFILE OFF)
set(CMAKE_COLOR_MAKEFILE ON)
Expand Down Expand Up @@ -60,6 +68,7 @@ set_source_files_properties(generated/abag.c PROPERTIES LANGUAGE CXX)
add_library(kinova_control
${PROJECT_SOURCE_DIR}/generated/abag.c
${PROJECT_SOURCE_DIR}/src/kinova_util.cpp
${PROJECT_SOURCE_DIR}/src/util.cpp
)

# find all CPP files in the ./executables directory and create a executable target
Expand All @@ -71,5 +80,5 @@ foreach ( SRC_FILE ${EXE_LIST} )

MESSAGE("creating TARGET_EXE_NAME: '${TARGET_EXE_NAME}'")
add_executable(${TARGET_EXE_NAME} ${SRC_FILE})
target_link_libraries(${TARGET_EXE_NAME} kinova_control)
target_link_libraries(${TARGET_EXE_NAME} kinova_control ${CONFIG_CPP_LIBRARY})
endforeach()
5 changes: 5 additions & 0 deletions config/abag.cfg
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
controllers = {
position_velocity = {
alpha = [ 0.9, 0.9, 0.95, 0.9, 0.9, 0.95 ]
};
};
30 changes: 15 additions & 15 deletions executables/control_kinova_pos.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,13 @@
#include <string>
#include <vector>
#include <math.h>
#include <stdlib.h> /* abs */
#include <chrono>
#include <time.h>
#include <unistd.h>

#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include "util.h"
#include "kinova_util.h"
#include "abag.h"
#include "constants.hpp"
Expand All @@ -23,6 +21,8 @@

namespace k_api = Kinova::Api;
namespace sc = std::chrono;
namespace kc = kinova_ctrl;
namespace kc_const = kinova_ctrl::constants;

#define IP_ADDRESS "192.168.1.10"
#define PORT 10000
Expand All @@ -36,10 +36,10 @@ void example_cyclic_torque_control (
k_api::ActuatorConfig::ActuatorConfigClient* actuator_config
)
{
const std::string UrdfPath = REPO_DIR + constants::kinova::URDF_PATH;
const std::string UrdfPath = REPO_DIR + kc_const::kinova::URDF_PATH;
std::cout << "loading URDF file at: " << UrdfPath << std::endl;
KDL::Tree kinovaTree; KDL::Chain kinovaChain;
loadUrdfModel(UrdfPath, kinovaTree, kinovaChain);
kc::loadUrdfModel(UrdfPath, kinovaTree, kinovaChain);
// Initialize solvers
const KDL::JntArray ZERO_ARRAY(7);
const KDL::Wrenches ZERO_WRENCHES(kinovaChain.getNrOfSegments(), KDL::Wrench::Zero());
Expand Down Expand Up @@ -227,9 +227,9 @@ void example_cyclic_torque_control (

// std::cout << jntCmdTorques.data.transpose() << std::endl;
// control
writeDataRow(logPosX, abagStatePos[0], totalElapsedTime.count(), errorPos[0], commandPos[0], endEffPose.p(0));
writeDataRow(logPosY, abagStatePos[1], totalElapsedTime.count(), errorPos[1], commandPos[1], endEffPose.p(1));
writeDataRow(logPosZ, abagStatePos[2], totalElapsedTime.count(), errorPos[2], commandPos[2], endEffPose.p(2));
kc::writeDataRow(logPosX, abagStatePos[0], totalElapsedTime.count(), errorPos[0], commandPos[0], endEffPose.p(0));
kc::writeDataRow(logPosY, abagStatePos[1], totalElapsedTime.count(), errorPos[1], commandPos[1], endEffPose.p(1));
kc::writeDataRow(logPosZ, abagStatePos[2], totalElapsedTime.count(), errorPos[2], commandPos[2], endEffPose.p(2));

// Incrementing identifier ensures actuators can reject out of time frames
base_command.set_frame_id(base_command.frame_id() + 1);
Expand All @@ -253,11 +253,11 @@ void example_cyclic_torque_control (
}

// Enforce the constant loop time and count how many times the loop was late
if (waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++;
if (kc::waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++;
}

// Set first actuator back in position
stopRobot(actuator_config);
kc::stopRobot(actuator_config);

// actuator_config->SetControlMode(control_mode_message, last_actuator_device_id);
std::cout << "Torque control example completed" << std::endl;
Expand All @@ -278,23 +278,23 @@ void example_cyclic_torque_control (

int main(int argc, char **argv)
{
KinovaBaseConnection connection(IP_ADDRESS, PORT, PORT_REAL_TIME, "admin", "kinova1_area4251");
kc::KinovaBaseConnection connection(IP_ADDRESS, PORT, PORT_REAL_TIME, "admin", "kinova1_area4251");

// Example core
move_to_home_position(connection.mBaseClient.get());
kc::move_to_home_position(connection.mBaseClient.get());
try
{
example_cyclic_torque_control(connection.mBaseClient.get(), connection.mBaseCyclicClient.get(), connection.mActuatorConfigClient.get());
}
catch (k_api::KDetailedException& ex)
{
stopRobot(connection.mActuatorConfigClient.get());
handleKinovaException(ex);
kc::stopRobot(connection.mActuatorConfigClient.get());
kc::handleKinovaException(ex);
return EXIT_FAILURE;
}
catch (std::exception& ex)
{
stopRobot(connection.mActuatorConfigClient.get());
kc::stopRobot(connection.mActuatorConfigClient.get());
std::cout << "Unhandled Error: " << ex.what() << std::endl;
return EXIT_FAILURE;
}
Expand Down
71 changes: 46 additions & 25 deletions executables/control_kinova_posvel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,42 +5,43 @@
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <stdlib.h> /* abs */
#include <chrono>
#include <time.h>
#include <unistd.h>
#include <libconfig.h++>

#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>

#include "util.h"
#include "kinova_util.h"
#include "abag.h"
#include "constants.hpp"

#define REPO_DIR "/home/minh/workspace/kinova_control_experiments/"
#define CTRL_APPROACH "position_velocity"

namespace k_api = Kinova::Api;
namespace sc = std::chrono;
namespace kc = kinova_ctrl;
namespace kc_const = kinova_ctrl::constants;

#define IP_ADDRESS "192.168.1.10"
#define PORT 10000
#define PORT_REAL_TIME 10001

std::chrono::duration <double, std::micro> loop_interval{};
sc::duration <double, std::micro> loop_interval{};

void example_cyclic_torque_control (
k_api::Base::BaseClient* base,
k_api::BaseCyclic::BaseCyclicClient* base_cyclic,
k_api::ActuatorConfig::ActuatorConfigClient* actuator_config
)
{
const std::string UrdfPath = REPO_DIR + constants::kinova::URDF_PATH;
const std::string UrdfPath = REPO_DIR + kc_const::kinova::URDF_PATH;
std::cout << "loading URDF file at: " << UrdfPath << std::endl;
KDL::Tree kinovaTree; KDL::Chain kinovaChain;
loadUrdfModel(UrdfPath, kinovaTree, kinovaChain);
kc::loadUrdfModel(UrdfPath, kinovaTree, kinovaChain);
// Initialize solvers
const KDL::JntArray ZERO_ARRAY(7);
const KDL::Wrenches ZERO_WRENCHES(kinovaChain.getNrOfSegments(), KDL::Wrench::Zero());
Expand Down Expand Up @@ -164,7 +165,7 @@ void example_cyclic_torque_control (
}
catch(...)
{
stopRobot(actuator_config);
kc::stopRobot(actuator_config);
if (desiredValuesLog.is_open()) desiredValuesLog.close();
if (logPosX.is_open()) logPosX.close();
if (logPosY.is_open()) logPosY.close();
Expand Down Expand Up @@ -256,12 +257,12 @@ void example_cyclic_torque_control (
}

// control
writeDataRow(logPosX, abagStates[0], totalElapsedTime.count(), errors[0], commands[0], endEffPose.p(0));
writeDataRow(logPosY, abagStates[1], totalElapsedTime.count(), errors[1], commands[1], endEffPose.p(1));
writeDataRow(logPosZ, abagStates[2], totalElapsedTime.count(), errors[2], commands[2], endEffPose.p(2));
writeDataRow(logVelX, abagStates[3], totalElapsedTime.count(), errors[3], commands[3], endEffTwist.p.v(0));
writeDataRow(logVelY, abagStates[4], totalElapsedTime.count(), errors[4], commands[4], endEffTwist.p.v(1));
writeDataRow(logVelZ, abagStates[5], totalElapsedTime.count(), errors[5], commands[5], endEffTwist.p.v(2));
kc::writeDataRow(logPosX, abagStates[0], totalElapsedTime.count(), errors[0], commands[0], endEffPose.p(0));
kc::writeDataRow(logPosY, abagStates[1], totalElapsedTime.count(), errors[1], commands[1], endEffPose.p(1));
kc::writeDataRow(logPosZ, abagStates[2], totalElapsedTime.count(), errors[2], commands[2], endEffPose.p(2));
kc::writeDataRow(logVelX, abagStates[3], totalElapsedTime.count(), errors[3], commands[3], endEffTwist.p.v(0));
kc::writeDataRow(logVelY, abagStates[4], totalElapsedTime.count(), errors[4], commands[4], endEffTwist.p.v(1));
kc::writeDataRow(logVelZ, abagStates[5], totalElapsedTime.count(), errors[5], commands[5], endEffTwist.p.v(2));

// Incrementing identifier ensures actuators can reject out of time frames
base_command.set_frame_id(base_command.frame_id() + 1);
Expand All @@ -276,7 +277,7 @@ void example_cyclic_torque_control (
}
catch(...)
{
stopRobot(actuator_config);
kc::stopRobot(actuator_config);
if (desiredValuesLog.is_open()) desiredValuesLog.close();
if (logPosX.is_open()) logPosX.close();
if (logPosY.is_open()) logPosY.close();
Expand All @@ -289,11 +290,11 @@ void example_cyclic_torque_control (
}

// Enforce the constant loop time and count how many times the loop was late
if (waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++;
if (kc::waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++;
}

// Set first actuator back in position
stopRobot(actuator_config);
kc::stopRobot(actuator_config);

// actuator_config->SetControlMode(control_mode_message, last_actuator_device_id);
std::cout << "Torque control example completed" << std::endl;
Expand All @@ -312,28 +313,48 @@ void example_cyclic_torque_control (
if (logVelZ.is_open()) logVelZ.close();

// Wait for a bit
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::this_thread::sleep_for(sc::milliseconds(2000));
}

int main(int argc, char **argv)
{
KinovaBaseConnection connection(IP_ADDRESS, PORT, PORT_REAL_TIME, "admin", "kinova1_area4251");
// Load configurations
libconfig::Config cfg; // this needs to exist to query the root setting, otherwise will cause segfault
try
{
libconfig::Setting& root = kc::loadConfigFile(cfg, REPO_DIR"config/abag.cfg");
std::vector<double> alphas;
kc::loadAbagConfig(root, CTRL_APPROACH, alphas);
for (auto& alpha : alphas) {
std::cout << alpha << std::endl;
}
}
catch (...)
{
std::cerr << "failed to load configuration file, exiting" << std::endl;
return EXIT_FAILURE;
}

// Establish connection with arm
kc::KinovaBaseConnection connection(IP_ADDRESS, PORT, PORT_REAL_TIME, "admin", "kinova1_area4251");

// Example core
move_to_home_position(connection.mBaseClient.get());
// Move to safe position
kc::move_to_home_position(connection.mBaseClient.get());
try
{
example_cyclic_torque_control(connection.mBaseClient.get(), connection.mBaseCyclicClient.get(), connection.mActuatorConfigClient.get());
// execute controller
example_cyclic_torque_control(
connection.mBaseClient.get(), connection.mBaseCyclicClient.get(), connection.mActuatorConfigClient.get());
}
catch (k_api::KDetailedException& ex)
{
stopRobot(connection.mActuatorConfigClient.get());
handleKinovaException(ex);
kc::stopRobot(connection.mActuatorConfigClient.get());
kc::handleKinovaException(ex);
return EXIT_FAILURE;
}
catch (std::exception& ex)
{
stopRobot(connection.mActuatorConfigClient.get());
kc::stopRobot(connection.mActuatorConfigClient.get());
std::cout << "Unhandled Error: " << ex.what() << std::endl;
return EXIT_FAILURE;
}
Expand Down
Loading

0 comments on commit 876d78a

Please sign in to comment.