From 633684289ede3e8ee6a65d7ad9ff7a4f991b3f1f Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Fri, 3 Jul 2020 20:25:52 +0200 Subject: [PATCH] add working pos-vel & vel control schemes * add velocity control scheme that lower the arm until hitting the table * add working versions of control scheme that use both position and velocity errors, i.e. spring damper * update plot scripts for velocity control --- executables/control_kinova_posvel.cpp | 341 ++++++++++++++++++++++++++ executables/control_kinova_vel.cpp | 324 ++++++++++++++++++++++++ executables/plot_abag_data.py | 11 +- 3 files changed, 675 insertions(+), 1 deletion(-) create mode 100644 executables/control_kinova_posvel.cpp create mode 100644 executables/control_kinova_vel.cpp diff --git a/executables/control_kinova_posvel.cpp b/executables/control_kinova_posvel.cpp new file mode 100644 index 0000000..1d21c65 --- /dev/null +++ b/executables/control_kinova_posvel.cpp @@ -0,0 +1,341 @@ +/** +* @copyright 2020 Minh Nguyen +*/ +#include +#include +#include +#include +#include +#include /* abs */ +#include +#include +#include + +#include +#include +#include +#include + +#include "kinova_util.h" +#include "abag.h" +#include "constants.hpp" + +#define REPO_DIR "/home/minh/workspace/kinova_control_experiments/" + +namespace k_api = Kinova::Api; +namespace sc = std::chrono; + +#define IP_ADDRESS "192.168.1.10" +#define PORT 10000 +#define PORT_REAL_TIME 10001 + +std::chrono::duration 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; + std::cout << "loading URDF file at: " << UrdfPath << std::endl; + KDL::Tree kinovaTree; KDL::Chain kinovaChain; + loadUrdfModel(UrdfPath, kinovaTree, kinovaChain); + // Initialize solvers + const KDL::JntArray ZERO_ARRAY(7); + const KDL::Wrenches ZERO_WRENCHES(kinovaChain.getNrOfSegments(), KDL::Wrench::Zero()); + + KDL::Jacobian jacobianEndEff(kinovaChain.getNrOfJoints()); + KDL::Frame endEffPose; + Eigen::Matrix endEffForce; + endEffForce.setZero(); + + int returnFlag = 0; + KDL::ChainJntToJacSolver jacobSolver(kinovaChain); + KDL::ChainFkSolverPos_recursive fkSolverPos(kinovaChain); + std::shared_ptr idSolver = std::make_shared(kinovaChain, + KDL::Vector(0.0, 0.0, -9.81289)); + + KDL::JntArray jntCmdTorques(7), jntPositions(7), jntVelocities(7), jnt_torque(7), jntImpedanceTorques(7); + + KDL::FrameVel endEffTwist; + KDL::ChainFkSolverVel_recursive fkSolverVel(kinovaChain); + KDL::JntArrayVel jntPositionVelocity(7); + + // Clearing faults + try + { + base->ClearFaults(); + } + catch(...) + { + std::cout << "Unable to clear robot faults" << std::endl; + throw; + } + + k_api::BaseCyclic::Feedback base_feedback; + k_api::BaseCyclic::Command base_command; + auto servoing_mode = k_api::Base::ServoingModeInformation(); + + std::cout << "Initializing the arm for torque control example" << std::endl; + + // Set the base in low-level servoing mode + try { + servoing_mode.set_servoing_mode(k_api::Base::ServoingMode::LOW_LEVEL_SERVOING); + base->SetServoingMode(servoing_mode); + base_feedback = base_cyclic->RefreshFeedback(); + } catch (...) { + std::cerr << "error setting low level control mode" << std::endl; + throw; + } + + // Initialize each actuator to their current position + // Save the current actuator position, to avoid a following error + for (int i = 0; i < ACTUATOR_COUNT; i++) + base_command.add_actuators()->set_position(base_feedback.actuators(i).position()); + + // Send a first frame + base_feedback = base_cyclic->Refresh(base_command); + + // Set last actuator in torque mode now that the command is equal to measure + auto control_mode_message = k_api::ActuatorConfig::ControlModeInformation(); + control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::TORQUE); + + for (int actuator_id = 1; actuator_id < ACTUATOR_COUNT + 1; actuator_id++) + actuator_config->SetControlMode(control_mode_message, actuator_id); + + const std::vector joint_torque_limits {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0}; + const std::vector cart_force_limit {5.0, 5.0, 5.0, 5.0, 5.0, 5.0}; // N + + // ABAG parameters + const double alphas[6] = { 0.9, 0.9, 0.95, 0.9, 0.9, 0.95 }; + const double biasThresholds[6] = { 0.000407, 0.000407, 0.000407, 0.000407, 0.000407, 0.000407 }; + const double biasSteps[6] = { 0.000400, 0.000400, 0.000400, 0.000400, 0.000400, 0.000400 }; + const double gainThresholds[6] = { 0.502492, 0.55, 0.550000, 0.502492, 0.55, 0.550000 }; + const double gainSteps[6] = { 0.002, 0.003, 0.003, 0.002, 0.003, 0.003 }; + double desiredPosistion[3] = { 0.0 }; + const double desiredTwist[3] = { 0.01, 0.01, 0.01 }; + double errors[6] = { 0.0 }; + double commands[6] = { 0.0 }; + abagState_t abagStates[6]; + for (int i = 0; i < 6; i++) { + initialize_abagState(&abagStates[i]); + } + + // Real-time loop + const int SECOND_IN_MICROSECONDS = 1000000; + const int RATE_HZ = 600; // Hz + const int TASK_TIME_LIMIT_SEC = 30; + const sc::microseconds TASK_TIME_LIMIT_MICRO(TASK_TIME_LIMIT_SEC * SECOND_IN_MICROSECONDS); + const sc::microseconds LOOP_DURATION(SECOND_IN_MICROSECONDS / RATE_HZ); + + int iterationCount = 0; + int slowLoopCount = 0; + sc::time_point controlStartTime = sc::steady_clock::now();; + sc::time_point loopStartTime = sc::steady_clock::now();; + sc::duration totalElapsedTime = loopStartTime - controlStartTime; + + // open log file streams + std::ofstream desiredValuesLog; + desiredValuesLog.open("desired_values.csv", std::ofstream::out | std::ofstream::trunc); + desiredValuesLog << "position X, position Y, position Z, velocity X, velocity Y, velocity Z" << std::endl; + std::ofstream logPosX, logPosY, logPosZ, logVelX, logVelY, logVelZ; + logPosX.open("ctrl_data_pos_x.csv", std::ofstream::out | std::ofstream::trunc); + logPosY.open("ctrl_data_pos_y.csv", std::ofstream::out | std::ofstream::trunc); + logPosZ.open("ctrl_data_pos_z.csv", std::ofstream::out | std::ofstream::trunc); + logVelX.open("ctrl_data_vel_x.csv", std::ofstream::out | std::ofstream::trunc); + logVelY.open("ctrl_data_vel_y.csv", std::ofstream::out | std::ofstream::trunc); + logVelZ.open("ctrl_data_vel_z.csv", std::ofstream::out | std::ofstream::trunc); + logPosX << "time (ns), error, signed, bias, gain, e_bar, command x, meas pos x" << std::endl; + logPosY << "time (ns), error, signed, bias, gain, e_bar, command y, meas pos y" << std::endl; + logPosZ << "time (ns), error, signed, bias, gain, e_bar, command z, meas pos z" << std::endl; + logVelX << "time (ns), error, signed, bias, gain, e_bar, command x, meas vel x" << std::endl; + logVelY << "time (ns), error, signed, bias, gain, e_bar, command y, meas vel y" << std::endl; + logVelZ << "time (ns), error, signed, bias, gain, e_bar, command z, meas vel z" << std::endl; + while (totalElapsedTime < TASK_TIME_LIMIT_MICRO) + { + iterationCount++; + loopStartTime = sc::steady_clock::now(); + totalElapsedTime = loopStartTime - controlStartTime; + + try + { + base_feedback = base_cyclic->RefreshFeedback(); + } + catch(...) + { + stopRobot(actuator_config); + if (desiredValuesLog.is_open()) desiredValuesLog.close(); + if (logPosX.is_open()) logPosX.close(); + if (logPosY.is_open()) logPosY.close(); + if (logPosZ.is_open()) logPosZ.close(); + if (logVelX.is_open()) logVelX.close(); + if (logVelY.is_open()) logVelY.close(); + if (logVelZ.is_open()) logVelZ.close(); + std::cerr << "error reading sensors" << std::endl; + throw; + } + + for (int i = 0; i < ACTUATOR_COUNT; i++) + { + jntPositions(i) = DEG_TO_RAD(base_feedback.actuators(i).position()); // deg + jntVelocities(i) = DEG_TO_RAD(base_feedback.actuators(i).velocity()); // deg + jnt_torque(i) = base_feedback.actuators(i).torque(); // nm + } + + // Kinova API provides only positive angle values + // This operation is required to align the logic with our safety monitor + // We need to convert some angles to negative values + if (jntPositions(1) > DEG_TO_RAD(180.0)) jntPositions(1) = jntPositions(1) - DEG_TO_RAD(360.0); + if (jntPositions(3) > DEG_TO_RAD(180.0)) jntPositions(3) = jntPositions(3) - DEG_TO_RAD(360.0); + if (jntPositions(5) > DEG_TO_RAD(180.0)) jntPositions(5) = jntPositions(5) - DEG_TO_RAD(360.0); + + // gravity compensation (zero velocity, acceleration & external wrench) + returnFlag = idSolver->CartToJnt(jntPositions, ZERO_ARRAY, ZERO_ARRAY, ZERO_WRENCHES, jntCmdTorques); + if (returnFlag != 0) break; + + // solve for end effector Jacobian and pose with respect to base frame + returnFlag = jacobSolver.JntToJac(jntPositions, jacobianEndEff); + if (returnFlag != 0) break; + returnFlag = fkSolverPos.JntToCart(jntPositions, endEffPose); + if (returnFlag != 0) break; + jntPositionVelocity.q = jntPositions; + jntPositionVelocity.qdot = jntVelocities; + returnFlag = fkSolverVel.JntToCart(jntPositionVelocity, endEffTwist); + if (returnFlag != 0) break; + + if (iterationCount == 1) + { + desiredPosistion[0] = endEffPose.p(0) + 0.10; + desiredPosistion[1] = endEffPose.p(1) + 0.10; + desiredPosistion[2] = endEffPose.p(2) + 0.10; + desiredValuesLog << desiredPosistion[0] << "," << desiredPosistion[1] << "," << desiredPosistion[2] << "," + << desiredTwist[0] << "," << desiredTwist[1] << "," << desiredTwist[2] << std::endl; + if (desiredValuesLog.is_open()) desiredValuesLog.close(); + } + + errors[0] = desiredPosistion[0] - endEffPose.p(0); + errors[1] = desiredPosistion[1] - endEffPose.p(1); + errors[2] = desiredPosistion[2] - endEffPose.p(2); + errors[3] = desiredTwist[0] - endEffTwist.p.v(0); + errors[4] = desiredTwist[1] - endEffTwist.p.v(1); + errors[5] = desiredTwist[2] - endEffTwist.p.v(2); + + abag_sched(&abagStates[0], &errors[0], &commands[0], &alphas[0], + &biasThresholds[0], &biasSteps[0], &gainThresholds[0], &gainSteps[0]); + abag_sched(&abagStates[1], &errors[1], &commands[1], &alphas[1], + &biasThresholds[1], &biasSteps[1], &gainThresholds[1], &gainSteps[1]); + abag_sched(&abagStates[2], &errors[2], &commands[2], &alphas[2], + &biasThresholds[2], &biasSteps[2], &gainThresholds[2], &gainSteps[2]); + abag_sched(&abagStates[3], &errors[3], &commands[3], &alphas[3], + &biasThresholds[3], &biasSteps[3], &gainThresholds[3], &gainSteps[3]); + abag_sched(&abagStates[4], &errors[4], &commands[4], &alphas[4], + &biasThresholds[4], &biasSteps[4], &gainThresholds[4], &gainSteps[4]); + abag_sched(&abagStates[5], &errors[5], &commands[5], &alphas[5], + &biasThresholds[5], &biasSteps[5], &gainThresholds[5], &gainSteps[5]); + + endEffForce(0) = commands[0] * cart_force_limit[0]; + endEffForce(1) = commands[1] * cart_force_limit[1]; + endEffForce(2) = commands[2] * cart_force_limit[2]; + + endEffForce(0) += commands[3] * cart_force_limit[3]; + endEffForce(1) += commands[4] * cart_force_limit[4]; + endEffForce(2) += commands[5] * cart_force_limit[5]; + + // std::cout << "ee force: " << endEffForce(0) << ", " << endEffForce(1) << ", " << endEffForce(2) << std::endl; + + jntImpedanceTorques.data = jacobianEndEff.data.transpose() * endEffForce; + + for (int i = 0; i < ACTUATOR_COUNT; i++) + { + jntCmdTorques(i) = jntCmdTorques(i) + jntImpedanceTorques(i); + if (jntCmdTorques(i) >= joint_torque_limits[i]) jntCmdTorques(i) = joint_torque_limits[i] - 0.001; + else if (jntCmdTorques(i) <= -joint_torque_limits[i]) jntCmdTorques(i) = -joint_torque_limits[i] + 0.001; + base_command.mutable_actuators(i)->set_position(base_feedback.actuators(i).position()); + base_command.mutable_actuators(i)->set_torque_joint(jntCmdTorques(i)); + } + + // 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)); + + // Incrementing identifier ensures actuators can reject out of time frames + base_command.set_frame_id(base_command.frame_id() + 1); + if (base_command.frame_id() > 65535) base_command.set_frame_id(0); + + for (int idx = 0; idx < ACTUATOR_COUNT; idx++) + base_command.mutable_actuators(idx)->set_command_id(base_command.frame_id()); + + try + { + base_feedback = base_cyclic->Refresh(base_command, 0); + } + catch(...) + { + stopRobot(actuator_config); + if (desiredValuesLog.is_open()) desiredValuesLog.close(); + if (logPosX.is_open()) logPosX.close(); + if (logPosY.is_open()) logPosY.close(); + if (logPosZ.is_open()) logPosZ.close(); + if (logVelX.is_open()) logVelX.close(); + if (logVelY.is_open()) logVelY.close(); + if (logVelZ.is_open()) logVelZ.close(); + std::cerr << "error sending command" << std::endl; + throw; + } + + // Enforce the constant loop time and count how many times the loop was late + if (waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++; + } + + // Set first actuator back in position + stopRobot(actuator_config); + + // actuator_config->SetControlMode(control_mode_message, last_actuator_device_id); + std::cout << "Torque control example completed" << std::endl; + std::cout << "Number of loops which took longer than specified duration: " << slowLoopCount << std::endl; + + // Set the servoing mode back to Single Level + servoing_mode.set_servoing_mode(k_api::Base::ServoingMode::SINGLE_LEVEL_SERVOING); + base->SetServoingMode(servoing_mode); + + // close log files + if (logPosX.is_open()) logPosX.close(); + if (logPosY.is_open()) logPosY.close(); + if (logPosZ.is_open()) logPosZ.close(); + if (logVelX.is_open()) logVelX.close(); + if (logVelY.is_open()) logVelY.close(); + if (logVelZ.is_open()) logVelZ.close(); + + // Wait for a bit + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +int main(int argc, char **argv) +{ + KinovaBaseConnection connection(IP_ADDRESS, PORT, PORT_REAL_TIME, "admin", "kinova1_area4251"); + + // Example core + 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); + return EXIT_FAILURE; + } + catch (std::exception& ex) + { + stopRobot(connection.mActuatorConfigClient.get()); + std::cout << "Unhandled Error: " << ex.what() << std::endl; + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} diff --git a/executables/control_kinova_vel.cpp b/executables/control_kinova_vel.cpp new file mode 100644 index 0000000..657816c --- /dev/null +++ b/executables/control_kinova_vel.cpp @@ -0,0 +1,324 @@ +/** +* @copyright 2020 Minh Nguyen +*/ +#include +#include +#include +#include +#include +#include /* abs */ +#include +#include +#include + +#include +#include +#include +#include + +#include "kinova_util.h" +#include "abag.h" +#include "constants.hpp" + +#define REPO_DIR "/home/minh/workspace/kinova_control_experiments/" + +namespace k_api = Kinova::Api; +namespace sc = std::chrono; + +#define IP_ADDRESS "192.168.1.10" +#define PORT 10000 +#define PORT_REAL_TIME 10001 + +std::chrono::duration 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; + std::cout << "loading URDF file at: " << UrdfPath << std::endl; + KDL::Tree kinovaTree; KDL::Chain kinovaChain; + loadUrdfModel(UrdfPath, kinovaTree, kinovaChain); + // Initialize solvers + const KDL::JntArray ZERO_ARRAY(7); + const KDL::Wrenches ZERO_WRENCHES(kinovaChain.getNrOfSegments(), KDL::Wrench::Zero()); + + KDL::Jacobian jacobianEndEff(kinovaChain.getNrOfJoints()); + KDL::Frame endEffPose; + Eigen::Matrix endEffForce; + endEffForce.setZero(); + + int returnFlag = 0; + KDL::ChainJntToJacSolver jacobSolver(kinovaChain); + KDL::ChainFkSolverPos_recursive fkSolverPos(kinovaChain); + std::shared_ptr idSolver = std::make_shared(kinovaChain, + KDL::Vector(0.0, 0.0, -9.81289)); + + KDL::JntArray jntCmdTorques(7), jntPositions(7), jntVelocities(7), jnt_torque(7), jntImpedanceTorques(7); + + KDL::FrameVel endEffTwist; + KDL::ChainFkSolverVel_recursive fkSolverVel(kinovaChain); + KDL::JntArrayVel jntPositionVelocity(7); + double desiredTwist[3] = { 0.0, 0.0, -0.02 }; + + // Clearing faults + try + { + base->ClearFaults(); + } + catch(...) + { + std::cout << "Unable to clear robot faults" << std::endl; + throw; + } + + k_api::BaseCyclic::Feedback base_feedback; + k_api::BaseCyclic::Command base_command; + auto servoing_mode = k_api::Base::ServoingModeInformation(); + + std::cout << "Initializing the arm for torque control example" << std::endl; + + // Set the base in low-level servoing mode + try { + servoing_mode.set_servoing_mode(k_api::Base::ServoingMode::LOW_LEVEL_SERVOING); + base->SetServoingMode(servoing_mode); + base_feedback = base_cyclic->RefreshFeedback(); + } catch (...) { + std::cerr << "error setting low level control mode" << std::endl; + throw; + } + + // Initialize each actuator to their current position + // Save the current actuator position, to avoid a following error + for (int i = 0; i < ACTUATOR_COUNT; i++) + base_command.add_actuators()->set_position(base_feedback.actuators(i).position()); + + // Send a first frame + base_feedback = base_cyclic->Refresh(base_command); + + // Set last actuator in torque mode now that the command is equal to measure + auto control_mode_message = k_api::ActuatorConfig::ControlModeInformation(); + control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::TORQUE); + + for (int actuator_id = 1; actuator_id < ACTUATOR_COUNT + 1; actuator_id++) + actuator_config->SetControlMode(control_mode_message, actuator_id); + + const std::vector joint_torque_limits {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0}; + const std::vector cart_force_limit {5.0, 5.0, 5.0, 5.0, 5.0, 5.0}; // N + + // ABAG parameters + const double alphaPosition[3] = { 0.9, 0.9, 0.95 }; + const double biasThresPos[3] = { 0.000407, 0.000407, 0.000407 }; + const double biasStepPos[3] = { 0.000400, 0.000400, 0.000400 }; + const double gainThresPos[3] = { 0.502492, 0.55, 0.550000 }; + const double gainStepPos[3] = { 0.002, 0.003, 0.003 }; + double desiredPosistion[3] = { 0.0 }; + double errorPos[3] = { 0.0 }; + double commandPos[3] = { 0.0 }; + abagState_t abagStatePos[3]; + for (int i = 0; i < 3; i++) { + initialize_abagState(&abagStatePos[i]); + } + + // Real-time loop + const int SECOND_IN_MICROSECONDS = 1000000; + const int RATE_HZ = 600; // Hz + const int TASK_TIME_LIMIT_SEC = 30; + const sc::microseconds TASK_TIME_LIMIT_MICRO(TASK_TIME_LIMIT_SEC * SECOND_IN_MICROSECONDS); + const sc::microseconds LOOP_DURATION(SECOND_IN_MICROSECONDS / RATE_HZ); + + int iterationCount = 0; + int slowLoopCount = 0; + sc::time_point controlStartTime = sc::steady_clock::now();; + sc::time_point loopStartTime = sc::steady_clock::now();; + sc::duration totalElapsedTime = loopStartTime - controlStartTime; + + // open log file streams + std::ofstream desiredPositionLog; + desiredPositionLog.open("desired_values.csv", std::ofstream::out | std::ofstream::trunc); + desiredPositionLog << "position X, position Y, position Z" << std::endl; + std::ofstream logPosX, logPosY, logPosZ; + logPosX.open("ctrl_data_pos_x.csv", std::ofstream::out | std::ofstream::trunc); + logPosY.open("ctrl_data_pos_y.csv", std::ofstream::out | std::ofstream::trunc); + logPosZ.open("ctrl_data_pos_z.csv", std::ofstream::out | std::ofstream::trunc); + logPosX << "time (ns), error, signed, bias, gain, e_bar, command x, meas pos x" << std::endl; + logPosY << "time (ns), error, signed, bias, gain, e_bar, command y, meas pos y" << std::endl; + logPosZ << "time (ns), error, signed, bias, gain, e_bar, command z, meas pos z" << std::endl; + while (totalElapsedTime < TASK_TIME_LIMIT_MICRO) + { + iterationCount++; + loopStartTime = sc::steady_clock::now(); + totalElapsedTime = loopStartTime - controlStartTime; + + try + { + base_feedback = base_cyclic->RefreshFeedback(); + } + catch(...) + { + stopRobot(actuator_config); + if (desiredPositionLog.is_open()) desiredPositionLog.close(); + if (logPosX.is_open()) logPosX.close(); + if (logPosY.is_open()) logPosY.close(); + if (logPosZ.is_open()) logPosZ.close(); + std::cerr << "error reading sensors" << std::endl; + throw; + } + + for (int i = 0; i < ACTUATOR_COUNT; i++) + { + jntPositions(i) = DEG_TO_RAD(base_feedback.actuators(i).position()); // deg + jntVelocities(i) = DEG_TO_RAD(base_feedback.actuators(i).velocity()); // deg + jnt_torque(i) = base_feedback.actuators(i).torque(); // nm + } + + if (jnt_torque(5) > -0.4) { + std::cout << "torques: joint 2: " << jnt_torque(1) << ", joint 4: " << jnt_torque(3) + << ", joint 6: " << jnt_torque(5) << std::endl; + break; + } + + // Kinova API provides only positive angle values + // This operation is required to align the logic with our safety monitor + // We need to convert some angles to negative values + if (jntPositions(1) > DEG_TO_RAD(180.0)) jntPositions(1) = jntPositions(1) - DEG_TO_RAD(360.0); + if (jntPositions(3) > DEG_TO_RAD(180.0)) jntPositions(3) = jntPositions(3) - DEG_TO_RAD(360.0); + if (jntPositions(5) > DEG_TO_RAD(180.0)) jntPositions(5) = jntPositions(5) - DEG_TO_RAD(360.0); + + // gravity compensation (zero velocity, acceleration & external wrench) + returnFlag = idSolver->CartToJnt(jntPositions, ZERO_ARRAY, ZERO_ARRAY, ZERO_WRENCHES, jntCmdTorques); + if (returnFlag != 0) break; + + // solve for end effector Jacobian and pose with respect to base frame + returnFlag = jacobSolver.JntToJac(jntPositions, jacobianEndEff); + if (returnFlag != 0) break; + // returnFlag = fkSolverPos.JntToCart(jntPositions, endEffPose); + // if (returnFlag != 0) break; + jntPositionVelocity.q = jntPositions; + jntPositionVelocity.qdot = jntVelocities; + returnFlag = fkSolverVel.JntToCart(jntPositionVelocity, endEffTwist); + if (returnFlag != 0) break; + + // returnFlag = fk_solver_vel.JntToCart(jntPositions, jnt_vel_, end_eff_twist); + // if (returnFlag != 0) break; + + if (iterationCount == 1) + { + // desiredPosistion[0] = endEffPose.p(0) + 0.03; + // desiredPosistion[1] = endEffPose.p(1) + 0.03; + // desiredPosistion[2] = endEffPose.p(2) + 0.03; + // desiredPositionLog << desiredPosistion[0] << "," << desiredPosistion[1] << "," + // << desiredPosistion[2] << std::endl; + desiredPositionLog << desiredTwist[0] << "," << desiredTwist[1] << "," << desiredTwist[2] << std::endl; + if (desiredPositionLog.is_open()) desiredPositionLog.close(); + } + + // errorPos[0] = desiredPosistion[0] - endEffPose.p(0); + // errorPos[1] = desiredPosistion[1] - endEffPose.p(1); + // errorPos[2] = desiredPosistion[2] - endEffPose.p(2); + errorPos[0] = desiredTwist[0] - endEffTwist.p.v(0); + errorPos[1] = desiredTwist[1] - endEffTwist.p.v(1); + errorPos[2] = desiredTwist[2] - endEffTwist.p.v(2); + + abag_sched(&abagStatePos[0], &errorPos[0], &commandPos[0], &alphaPosition[0], + &biasThresPos[0], &biasStepPos[0], &gainThresPos[0], &gainStepPos[0]); + abag_sched(&abagStatePos[1], &errorPos[1], &commandPos[1], &alphaPosition[1], + &biasThresPos[1], &biasStepPos[1], &gainThresPos[1], &gainStepPos[1]); + abag_sched(&abagStatePos[2], &errorPos[2], &commandPos[2], &alphaPosition[2], + &biasThresPos[2], &biasStepPos[2], &gainThresPos[2], &gainStepPos[2]); + + endEffForce(0) = commandPos[0] * cart_force_limit[0]; + endEffForce(1) = commandPos[1] * cart_force_limit[1]; + endEffForce(2) = commandPos[2] * cart_force_limit[2]; + + jntImpedanceTorques.data = jacobianEndEff.data.transpose() * endEffForce; + + for (int i = 0; i < ACTUATOR_COUNT; i++) + { + jntCmdTorques(i) = jntCmdTorques(i) + jntImpedanceTorques(i); + if (jntCmdTorques(i) >= joint_torque_limits[i]) jntCmdTorques(i) = joint_torque_limits[i] - 0.001; + else if (jntCmdTorques(i) <= -joint_torque_limits[i]) jntCmdTorques(i) = -joint_torque_limits[i] + 0.001; + base_command.mutable_actuators(i)->set_position(base_feedback.actuators(i).position()); + base_command.mutable_actuators(i)->set_torque_joint(jntCmdTorques(i)); + } + + // 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)); + writeDataRow(logPosX, abagStatePos[0], totalElapsedTime.count(), errorPos[0], commandPos[0], endEffTwist.p.v(0)); + writeDataRow(logPosY, abagStatePos[1], totalElapsedTime.count(), errorPos[1], commandPos[1], endEffTwist.p.v(1)); + writeDataRow(logPosZ, abagStatePos[2], totalElapsedTime.count(), errorPos[2], commandPos[2], endEffTwist.p.v(2)); + + // Incrementing identifier ensures actuators can reject out of time frames + base_command.set_frame_id(base_command.frame_id() + 1); + if (base_command.frame_id() > 65535) base_command.set_frame_id(0); + + for (int idx = 0; idx < ACTUATOR_COUNT; idx++) + base_command.mutable_actuators(idx)->set_command_id(base_command.frame_id()); + + try + { + base_feedback = base_cyclic->Refresh(base_command, 0); + } + catch(...) + { + stopRobot(actuator_config); + if (desiredPositionLog.is_open()) desiredPositionLog.close(); + if (logPosX.is_open()) logPosX.close(); + if (logPosY.is_open()) logPosY.close(); + if (logPosZ.is_open()) logPosZ.close(); + std::cerr << "error sending command" << std::endl; + throw; + } + + // Enforce the constant loop time and count how many times the loop was late + if (waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++; + } + + // Set first actuator back in position + stopRobot(actuator_config); + + // actuator_config->SetControlMode(control_mode_message, last_actuator_device_id); + std::cout << "Torque control example completed" << std::endl; + std::cout << "Number of loops which took longer than specified duration: " << slowLoopCount << std::endl; + + // Set the servoing mode back to Single Level + servoing_mode.set_servoing_mode(k_api::Base::ServoingMode::SINGLE_LEVEL_SERVOING); + base->SetServoingMode(servoing_mode); + + // close log files + if (logPosX.is_open()) logPosX.close(); + if (logPosY.is_open()) logPosY.close(); + if (logPosZ.is_open()) logPosZ.close(); + + // Wait for a bit + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +} + +int main(int argc, char **argv) +{ + KinovaBaseConnection connection(IP_ADDRESS, PORT, PORT_REAL_TIME, "admin", "kinova1_area4251"); + + // Example core + 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); + return EXIT_FAILURE; + } + catch (std::exception& ex) + { + stopRobot(connection.mActuatorConfigClient.get()); + std::cout << "Unhandled Error: " << ex.what() << std::endl; + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} diff --git a/executables/plot_abag_data.py b/executables/plot_abag_data.py index 97542c7..975b986 100755 --- a/executables/plot_abag_data.py +++ b/executables/plot_abag_data.py @@ -14,6 +14,9 @@ class ColumnsDesired(Enum): POS_X = 0 POS_Y = 1 POS_Z = 2 + VEL_X = 3 + VEL_Y = 4 + VEL_Z = 5 def get_column(name): if name == 'pos_x': @@ -22,6 +25,12 @@ def get_column(name): return ColumnsDesired.POS_Y.value elif name == 'pos_z': return ColumnsDesired.POS_Z.value + if name == 'vel_x': + return ColumnsDesired.VEL_X.value + elif name == 'vel_y': + return ColumnsDesired.VEL_Y.value + elif name == 'vel_z': + return ColumnsDesired.VEL_Z.value else: raise ValueError("invalid column name: " + name) @@ -117,7 +126,7 @@ def plot_csv_data(csv_file_path, desired_file, ctrl_dimension): parser = argparse.ArgumentParser(description="simple script to plot ABAG control data") parser.add_argument("csv_file", help="CSV file containing ABAG control data") parser.add_argument("desired_file", help="CSV file containing desired values") - parser.add_argument("ctrl_dimension", choices=['pos_x', 'pos_y', 'pos_z'], + parser.add_argument("ctrl_dimension", choices=['pos_x', 'pos_y', 'pos_z', 'vel_x', 'vel_y', 'vel_z'], help="specify which dimension is being controlled in data") args = parser.parse_args() try: