From b9379e0de83fcd58c671c5219bb9c0bbc2525df8 Mon Sep 17 00:00:00 2001 From: Tai Hoang Date: Tue, 20 Jul 2021 17:56:21 +0200 Subject: [PATCH 01/10] Fixed Jacobian matrices to make sure L is a block diagonal matrix --- src/kinematics.cpp | 79 ++++++++++++++++++++++++++++++++-------------- 1 file changed, 56 insertions(+), 23 deletions(-) diff --git a/src/kinematics.cpp b/src/kinematics.cpp index e606227..89b3cc4 100644 --- a/src/kinematics.cpp +++ b/src/kinematics.cpp @@ -228,20 +228,20 @@ void Kinematics::update_V() { segmentVector = segment.second->global_coordinates - segment.first->global_coordinates; segmentVector.normalize(); - int k = segment.first->link_index; - if (k > 0) { - // Total V term in translations - Vector3d V_ijk_T = -world_to_link_transform[k].block(0, 0, 3, 3) * segmentVector; - - Vector3d temp_vec2 = segment.first->local_coordinates; - - Vector3d V_itk_T = temp_vec2.cross(V_ijk_T); - - V.block(muscle_index, 6 * k, 1, 3) = V_ijk_T.transpose(); - V.block(muscle_index, 6 * k + 3, 1, 3) = V_itk_T.transpose(); - } +// int k = segment.first->link_index; +// if (k > 0) { +// // Total V term in translations +// Vector3d V_ijk_T = -world_to_link_transform[k].block(0, 0, 3, 3) * segmentVector; +// +// Vector3d temp_vec2 = segment.first->local_coordinates; +// +// Vector3d V_itk_T = temp_vec2.cross(V_ijk_T); +// +// V.block(muscle_index, 6 * k, 1, 3) = V_ijk_T.transpose(); +// V.block(muscle_index, 6 * k + 3, 1, 3) = V_itk_T.transpose(); +// } - k = segment.second->link_index; + int k = segment.second->link_index; if (k > 0) { // Total V term in translations Vector3d V_ijk_T = world_to_link_transform[k].block(0, 0, 3, 3) * segmentVector; @@ -262,11 +262,39 @@ void Kinematics::update_V() { void Kinematics::update_S() { S.setZero(6 * number_of_links, number_of_dofs); - int k = 1; - for (auto &axis:joint_axis) { - S.block(6 * k, k - 1, 6, 1) = axis; - k++; + + vector link_names_local = {"upperarm_left", "lowerarm_left", "hand_left", + "head", + "upperarm_right", "lowerarm_right", "hand_right", + }; + vector> joint_names_local = { + {"shoulder_left_axis0", "shoulder_left_axis1", "shoulder_left_axis2"},{"elbow_left_axis0","elbow_left_axis1"},{"wrist_left_axis0", "wrist_left_axis1", "wrist_left_axis2"}, + {"head_axis0", "head_axis1", "head_axis2"}, + {"shoulder_right_axis0", "shoulder_right_axis1", "shoulder_right_axis2"},{"elbow_right_axis0","elbow_right_axis1"},{"wrist_right_axis0", "wrist_right_axis1", "wrist_right_axis2"}, + }; + + int k = 0; + for (int i=0; i < link_names_local.size(); i++){ + + int link_idx = -1; + for (int l=0; l < link_names.size(); l++){ + if(link_names[l] == link_names_local[i]){ + link_idx = l; + break; + } + } + + for (int j=0; j < joint_names_local[i].size(); j++){ + S.block(6 * link_idx, k, 6, 1) = joint_axis[k]; + k++; + } } + +// int k = 1; +// for (auto &axis:joint_axis) { +// S.block(6 * k, k - 1, 6, 1) = axis; +// k++; +// } // ROS_INFO_STREAM("S_t = " << S.transpose().format(fmt)); } @@ -282,9 +310,14 @@ void Kinematics::update_P() { static int counter = 0; // #pragma omp parallel for for (int k = 1; k < number_of_links; k++) { - Matrix4d transformMatrix_k = world_to_link_transform[k]; - Matrix3d R_k0 = transformMatrix_k.block(0, 0, 3, 3); - for (int a = 1; a <= k; a++) { + if(link_names[k] == "head" || + link_names[k] == "upperarm_right" || link_names[k] == "lowerarm_right" || link_names[k] == "hand_right" || + link_names[k] == "upperarm_left" || link_names[k] == "lowerarm_left" || link_names[k] == "hand_left") { + + Matrix4d transformMatrix_k = world_to_link_transform[k]; + Matrix3d R_k0 = transformMatrix_k.block(0, 0, 3, 3); + + int a = k; Matrix4d transformMatrix_a = world_to_link_transform[a]; Matrix3d R_0a = transformMatrix_a.block(0, 0, 3, 3).transpose(); R_ka = R_k0 * R_0a; @@ -297,10 +330,10 @@ void Kinematics::update_P() { // absolute joint location Matrix4d pose = iDynTree::toEigen(kinDynComp.getWorldTransform(a).asHomogeneousTransform()); - r_OP = pose.topRightCorner(3,1); + r_OP = pose.topRightCorner(3, 1); // absolute com location - r_OG = link_to_world_transform[k].topRightCorner(3,1); + r_OG = link_to_world_transform[k].topRightCorner(3, 1); Matrix3d PaK_2_1 = EigenExtension::SkewSymmetric2(-r_OP + R_ka.transpose() * r_OG); Matrix3d PaK_2 = -R_ka * PaK_2_1; @@ -326,7 +359,7 @@ vector Kinematics::oneStepForward(double dt, VectorXd& q_in, VectorXd& for(int i = 0; i Date: Wed, 21 Jul 2021 16:02:58 +0200 Subject: [PATCH 02/10] Resolved comments --- CMakeLists.txt | 2 +- include/kindyn/kinematics.hpp | 3 +++ launch/robot.launch | 1 + src/kinematics.cpp | 40 ++++------------------------------- src/robot.cpp | 9 +++++++- src/robots/upper_body.cpp | 12 +++++------ 6 files changed, 23 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c6788b4..1fc21dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,7 +111,7 @@ add_library(kindyn SHARED include/kindyn/cable.hpp) add_dependencies(kindyn roboy_simulation_msgs_generate_messages_cpp roboy_simulation_msgs_msgs_generate_messages_cpp) target_link_libraries(kindyn ${catkin_LIBRARIES} ${iDynTree_LIBRARIES} ${qpOASES_LIBRARIES} CardsflowHardwareInterface - CableLengthController TorquePositionController ForcePositionController ${OpenMP_CXX_libraries} ${BFL_LIBRARIES}) + CableLengthController TorquePositionController ForcePositionController ${OpenMP_CXX_libraries}) # vrpuppet library add_library(vrpuppet SHARED diff --git a/include/kindyn/kinematics.hpp b/include/kindyn/kinematics.hpp index 422bd1a..d413cf6 100644 --- a/include/kindyn/kinematics.hpp +++ b/include/kindyn/kinematics.hpp @@ -71,6 +71,9 @@ namespace cardsflow { size_t number_of_cables = 0; /// number of cables, ie muscles of the whole robot size_t number_of_links = 0; /// number of links of the whole robot + vector link_relation_name; + vector> joint_relation_name; + vector cables; /// all cables of the robot vector joint_axis; /// joint axis of each joint vector link_names, joint_names; /// link and joint names of the robot diff --git a/launch/robot.launch b/launch/robot.launch index 2493fbc..71535c4 100644 --- a/launch/robot.launch +++ b/launch/robot.launch @@ -37,6 +37,7 @@ + [0.0, 0.0, 0.0] 5.0 5.0 diff --git a/src/kinematics.cpp b/src/kinematics.cpp index 89b3cc4..6df9bbf 100644 --- a/src/kinematics.cpp +++ b/src/kinematics.cpp @@ -228,19 +228,6 @@ void Kinematics::update_V() { segmentVector = segment.second->global_coordinates - segment.first->global_coordinates; segmentVector.normalize(); -// int k = segment.first->link_index; -// if (k > 0) { -// // Total V term in translations -// Vector3d V_ijk_T = -world_to_link_transform[k].block(0, 0, 3, 3) * segmentVector; -// -// Vector3d temp_vec2 = segment.first->local_coordinates; -// -// Vector3d V_itk_T = temp_vec2.cross(V_ijk_T); -// -// V.block(muscle_index, 6 * k, 1, 3) = V_ijk_T.transpose(); -// V.block(muscle_index, 6 * k + 3, 1, 3) = V_itk_T.transpose(); -// } - int k = segment.second->link_index; if (k > 0) { // Total V term in translations @@ -263,39 +250,22 @@ void Kinematics::update_V() { void Kinematics::update_S() { S.setZero(6 * number_of_links, number_of_dofs); - vector link_names_local = {"upperarm_left", "lowerarm_left", "hand_left", - "head", - "upperarm_right", "lowerarm_right", "hand_right", - }; - vector> joint_names_local = { - {"shoulder_left_axis0", "shoulder_left_axis1", "shoulder_left_axis2"},{"elbow_left_axis0","elbow_left_axis1"},{"wrist_left_axis0", "wrist_left_axis1", "wrist_left_axis2"}, - {"head_axis0", "head_axis1", "head_axis2"}, - {"shoulder_right_axis0", "shoulder_right_axis1", "shoulder_right_axis2"},{"elbow_right_axis0","elbow_right_axis1"},{"wrist_right_axis0", "wrist_right_axis1", "wrist_right_axis2"}, - }; - int k = 0; - for (int i=0; i < link_names_local.size(); i++){ + for (int i=0; i < link_relation_name.size(); i++){ int link_idx = -1; for (int l=0; l < link_names.size(); l++){ - if(link_names[l] == link_names_local[i]){ + if(link_names[l] == link_relation_name[i]){ link_idx = l; break; } } - for (int j=0; j < joint_names_local[i].size(); j++){ + for (int j=0; j < joint_relation_name[i].size(); j++){ S.block(6 * link_idx, k, 6, 1) = joint_axis[k]; k++; } } - -// int k = 1; -// for (auto &axis:joint_axis) { -// S.block(6 * k, k - 1, 6, 1) = axis; -// k++; -// } -// ROS_INFO_STREAM("S_t = " << S.transpose().format(fmt)); } void Kinematics::update_P() { @@ -310,9 +280,7 @@ void Kinematics::update_P() { static int counter = 0; // #pragma omp parallel for for (int k = 1; k < number_of_links; k++) { - if(link_names[k] == "head" || - link_names[k] == "upperarm_right" || link_names[k] == "lowerarm_right" || link_names[k] == "hand_right" || - link_names[k] == "upperarm_left" || link_names[k] == "lowerarm_left" || link_names[k] == "hand_left") { + if(std::find(link_relation_name.begin(), link_relation_name.end(), link_names[k]) != link_relation_name.end()) { Matrix4d transformMatrix_k = world_to_link_transform[k]; Matrix3d R_k0 = transformMatrix_k.block(0, 0, 3, 3); diff --git a/src/robot.cpp b/src/robot.cpp index bd7868a..3042f6c 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -51,7 +51,14 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vectorgetParam("external_robot_target", external_robot_target); nh->getParam("external_robot_state", external_robot_state); - nh->setParam("vr_puppet",false); + + nh->getParam("link_relation_names", kinematics.link_relation_name); + for (string lr:kinematics.link_relation_name) { + ROS_INFO_STREAM(lr); + vector joint_names; + nh->getParam(("link_relation/" + lr + "/joint_names"), joint_names); + kinematics.joint_relation_name.push_back(joint_names); + } kinematics.init(urdf_file_path, viapoints_file_path, joint_names_ordered); diff --git a/src/robots/upper_body.cpp b/src/robots/upper_body.cpp index e63543e..73a5711 100644 --- a/src/robots/upper_body.cpp +++ b/src/robots/upper_body.cpp @@ -40,7 +40,7 @@ class UpperBody: public cardsflow::kindyn::Robot{ map init_called; boost::shared_ptr system_status_thread; ros::Time prev_roboy_state_time; - + enum BulletPublish {zeroes, current}; public: /** * Constructor @@ -272,7 +272,7 @@ class UpperBody: public cardsflow::kindyn::Robot{ if(this->external_robot_state) { // Set current state to bullet - publishBulletTarget(name, "current"); + publishBulletTarget(name, BulletPublish::current); t0 = ros::Time::now(); int seconds = 3; @@ -281,7 +281,7 @@ class UpperBody: public cardsflow::kindyn::Robot{ } // Move back to zero position - publishBulletTarget(name, "zeroes"); + publishBulletTarget(name, BulletPublish::zeroes); } ROS_INFO_STREAM("%s pose init done" << name); @@ -298,7 +298,7 @@ class UpperBody: public cardsflow::kindyn::Robot{ * @param body_part * @param zeroes_or_current will publish either "zeroes" or "current" as targets to Bullet */ - void publishBulletTarget(string body_part, string zeroes_or_current){ + void publishBulletTarget(string body_part, BulletPublish zeroes_or_current){ sensor_msgs::JointState target_msg; @@ -320,10 +320,10 @@ class UpperBody: public cardsflow::kindyn::Robot{ if (joint_index != iDynTree::JOINT_INVALID_INDEX) { target_msg.name.push_back(joint); - if(zeroes_or_current == "zeroes") { + if(zeroes_or_current == BulletPublish::zeroes) { target_msg.position.push_back(0); ROS_WARN_STREAM("Set target 0 for " << joint); - }else if(zeroes_or_current == "current"){ + }else if(zeroes_or_current == BulletPublish::current){ target_msg.position.push_back(q[joint_index]); ROS_WARN_STREAM("Set target " << q[joint_index] << " for " << joint); } From bf5b84b8abf4d68cc91d79b1574c535f6afc862f Mon Sep 17 00:00:00 2001 From: Tai Hoang Date: Fri, 6 Aug 2021 15:01:12 +0200 Subject: [PATCH 03/10] Added control parameter for each joint individualy --- .../cardsflow_command_interface.hpp | 4 +- include/kindyn/kinematics.hpp | 3 +- include/kindyn/robot.hpp | 3 +- launch/robot.launch | 4 +- src/controller/cableLengthController.cpp | 5 +- .../cardsflow_command_interface.cpp | 2 +- src/kinematics.cpp | 7 +-- src/main_node.cpp | 49 +++++++++++++++++++ src/robot.cpp | 48 ++++++++++++------ 9 files changed, 97 insertions(+), 28 deletions(-) create mode 100644 src/main_node.cpp diff --git a/include/kindyn/controller/cardsflow_command_interface.hpp b/include/kindyn/controller/cardsflow_command_interface.hpp index bccd979..9257d3d 100644 --- a/include/kindyn/controller/cardsflow_command_interface.hpp +++ b/include/kindyn/controller/cardsflow_command_interface.hpp @@ -59,7 +59,7 @@ namespace hardware_interface { * @param motor_cmd cable command */ CardsflowHandle(const CardsflowStateHandle &js, double *joint_position_cmd, double *joint_velocity_cmd, - double* joint_torque_cmd, VectorXd *motor_cmd, double* Kp, double* Kd); + double* joint_torque_cmd, VectorXd *motor_cmd, VectorXd *Kp, VectorXd *Kd); /** * Cable length command @@ -103,7 +103,7 @@ namespace hardware_interface { */ void setJointTorqueCommand(double cmd); - double *Kp_, *Kd_; + VectorXd *Kp_, *Kd_; private: double *joint_position_cmd_, *joint_velocity_cmd_, *joint_torque_cmd_; /// joint position/velocity/torque command diff --git a/include/kindyn/kinematics.hpp b/include/kindyn/kinematics.hpp index d413cf6..45d6329 100644 --- a/include/kindyn/kinematics.hpp +++ b/include/kindyn/kinematics.hpp @@ -59,7 +59,7 @@ namespace cardsflow { /** * Do forward kinematics */ - vector oneStepForward(double dt, VectorXd& q_in, VectorXd& qd_in, vector Ld); + vector oneStepForward(VectorXd& q_in, VectorXd& qd_in, vector Ld); int GetJointIdByName(string joint); @@ -73,6 +73,7 @@ namespace cardsflow { vector link_relation_name; vector> joint_relation_name; + vector joint_dt; vector cables; /// all cables of the robot vector joint_axis; /// joint axis of each joint diff --git a/include/kindyn/robot.hpp b/include/kindyn/robot.hpp index 3fcd4a1..bddbc4b 100644 --- a/include/kindyn/robot.hpp +++ b/include/kindyn/robot.hpp @@ -150,7 +150,8 @@ namespace cardsflow { bool external_robot_target = false; bool debug_ = false; double k_dt = 0.005; - double *Kp_, *Kd_; + VectorXd Kp_, Kd_; + vector param_kp, param_kd; }; }; } diff --git a/launch/robot.launch b/launch/robot.launch index 71535c4..fc43ada 100644 --- a/launch/robot.launch +++ b/launch/robot.launch @@ -38,6 +38,7 @@ + [0.0, 0.0, 0.0] 5.0 5.0 @@ -45,9 +46,6 @@ - - - Kinematics::oneStepForward(double dt, VectorXd& q_in, VectorXd& qd_in, vector Ld) { +vector Kinematics::oneStepForward(VectorXd& q_in, VectorXd& qd_in, vector Ld) { + + integration_time = ros::Time::now().toSec(); for(int i=0;i Kinematics::oneStepForward(double dt, VectorXd& q_in, VectorXd& [this, j, qd_temp, dof_offset](const state_type &x, state_type &dxdt, double t) { dxdt[1] = 0; dxdt[0] = qd_temp[j-dof_offset]; - }, joint_state[j], integration_time, integration_time + dt, dt); + }, joint_state[j], integration_time, integration_time + joint_dt[j], joint_dt[j]); qd_next[j] = qd_temp[j-dof_offset]; q_next[j] = joint_state[j][0]; } @@ -354,7 +356,6 @@ vector Kinematics::oneStepForward(double dt, VectorXd& q_in, VectorXd& } } - integration_time += dt; ROS_INFO_THROTTLE(10, "forward kinematics calculated for %lf s", integration_time); vector result = {q_next, qd_next}; diff --git a/src/main_node.cpp b/src/main_node.cpp new file mode 100644 index 0000000..0410eff --- /dev/null +++ b/src/main_node.cpp @@ -0,0 +1,49 @@ +// +// Created by roboy on 01.07.21. +// + +int main(int argc, char *argv[]) { + + string robot_model(argv[1]); + ROS_INFO_STREAM("launching " << robot_model); + if (!ros::isInitialized()) { + int argc = 0; + char **argv = NULL; + ros::init(argc, argv, robot_model + "_upper_body"); + } + ros::NodeHandle nh; + string urdf, cardsflow_xml; + if(nh.hasParam("urdf_file_path") && nh.hasParam("cardsflow_xml")) { + nh.getParam("urdf_file_path", urdf); + nh.getParam("cardsflow_xml", cardsflow_xml); + }else { + ROS_FATAL("USAGE: rosrun kindyn test_robot path_to_urdf path_to_viapoints_xml"); + return 1; + } + ROS_INFO("\nurdf file path: %s\ncardsflow_xml %s", urdf.c_str(), cardsflow_xml.c_str()); + + + UpperBody robot(urdf, cardsflow_xml,robot_model); + controller_manager::ControllerManager cm(&robot); + + if (nh.hasParam("simulated")) { + nh.getParam("simulated", robot.simulated); + } + + thread update_thread(update, &cm); + update_thread.detach(); + + ros::Rate rate(200); + while(ros::ok()){ + robot.read(); + if (!robot.simulated) + robot.write(); + ros::spinOnce(); + rate.sleep(); + } + + ROS_INFO("TERMINATING..."); + update_thread.join(); + + return 0; +} diff --git a/src/robot.cpp b/src/robot.cpp index 3042f6c..927de77 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -98,14 +98,22 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vectorhasParam("k_dt")) - nh->getParam("k_dt", k_dt); - if (nh->hasParam("Kp")) - nh->getParam("Kp", *Kp_); - if (nh->hasParam("Kd")) - nh->getParam("Kd", *Kd_); + + kinematics.joint_dt.resize(kinematics.number_of_dofs); + Kp_.resize(kinematics.number_of_dofs); + Kd_.resize(kinematics.number_of_dofs); + param_kp.resize(kinematics.number_of_dofs); + param_kd.resize(kinematics.number_of_dofs); + + nh->getParam("joint_dt", kinematics.joint_dt); + nh->getParam("joint_kp", param_kp); + nh->getParam("joint_kd", param_kd); + for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { + Kp_[joint] = param_kp[joint]; + Kd_[joint] = param_kd[joint]; + ROS_INFO_STREAM(kinematics.joint_names[joint] << "\tdt=" << kinematics.joint_dt[joint] << + "\tkp=" << param_kp[joint] << "\tkd=" << param_kd[joint]); + } for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { ROS_INFO("initializing controllers for joint %d %s", joint, kinematics.joint_names[joint].c_str()); @@ -119,7 +127,7 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vectorhasParam("k_dt")) - nh->getParam("k_dt", k_dt); - if (nh->hasParam("Kp")) - nh->getParam("Kp", *Kp_); - if (nh->hasParam("Kd")) - nh->getParam("Kd", *Kd_); + if (nh->hasParam("joint_dt")) + nh->getParam("joint_dt", kinematics.joint_dt); + if (nh->hasParam("joint_kp")){ + nh->getParam("joint_kp", param_kp); + for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { + Kp_[joint] = param_kp[joint]; + } + } + if (nh->hasParam("joint_kd")) { + nh->getParam("joint_kd", param_kd); + for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { + Kd_[joint] = param_kd[joint]; + } + } } // TODO: Run the below code in critical section to avoid Mutex with the JointState and PROBABLY the controller @@ -217,7 +233,7 @@ void Robot::update(){ // ---------------------------------------------------------------------------- // Do one step forward Kinematics with current tendon velocity Ld and current state - vector state_next = kinematics.oneStepForward(k_dt, q, qd, Ld); + vector state_next = kinematics.oneStepForward(q, qd, Ld); if(!this->external_robot_state){ for(int i=0;i Date: Mon, 9 Aug 2021 15:59:59 +0200 Subject: [PATCH 04/10] Removed redundant file --- src/main_node.cpp | 49 ----------------------------------------------- 1 file changed, 49 deletions(-) delete mode 100644 src/main_node.cpp diff --git a/src/main_node.cpp b/src/main_node.cpp deleted file mode 100644 index 0410eff..0000000 --- a/src/main_node.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// -// Created by roboy on 01.07.21. -// - -int main(int argc, char *argv[]) { - - string robot_model(argv[1]); - ROS_INFO_STREAM("launching " << robot_model); - if (!ros::isInitialized()) { - int argc = 0; - char **argv = NULL; - ros::init(argc, argv, robot_model + "_upper_body"); - } - ros::NodeHandle nh; - string urdf, cardsflow_xml; - if(nh.hasParam("urdf_file_path") && nh.hasParam("cardsflow_xml")) { - nh.getParam("urdf_file_path", urdf); - nh.getParam("cardsflow_xml", cardsflow_xml); - }else { - ROS_FATAL("USAGE: rosrun kindyn test_robot path_to_urdf path_to_viapoints_xml"); - return 1; - } - ROS_INFO("\nurdf file path: %s\ncardsflow_xml %s", urdf.c_str(), cardsflow_xml.c_str()); - - - UpperBody robot(urdf, cardsflow_xml,robot_model); - controller_manager::ControllerManager cm(&robot); - - if (nh.hasParam("simulated")) { - nh.getParam("simulated", robot.simulated); - } - - thread update_thread(update, &cm); - update_thread.detach(); - - ros::Rate rate(200); - while(ros::ok()){ - robot.read(); - if (!robot.simulated) - robot.write(); - ros::spinOnce(); - rate.sleep(); - } - - ROS_INFO("TERMINATING..."); - update_thread.join(); - - return 0; -} From d14e0e83d0ffab2762cb6007a45f48de7a590b81 Mon Sep 17 00:00:00 2001 From: Tai Hoang Date: Fri, 13 Aug 2021 13:51:24 +0200 Subject: [PATCH 05/10] Fixed plexus time_stamp --- src/robots/upper_body.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/robots/upper_body.cpp b/src/robots/upper_body.cpp index 73a5711..d2e409c 100644 --- a/src/robots/upper_body.cpp +++ b/src/robots/upper_body.cpp @@ -359,6 +359,8 @@ class UpperBody: public cardsflow::kindyn::Robot{ } void MotorState(const roboy_middleware_msgs::MotorState::ConstPtr &msg){ + prev_roboy_state_time = ros::Time::now(); + int i=0; for (auto id:msg->global_id) { position[id] = msg->encoder0_pos[i]; @@ -374,7 +376,7 @@ class UpperBody: public cardsflow::kindyn::Robot{ } void RoboyState(const roboy_middleware_msgs::RoboyState::ConstPtr &msg) { - prev_roboy_state_time = msg->header.stamp; +// prev_roboy_state_time = ros::Time::now(); } void MotorInfo(const roboy_middleware_msgs::MotorInfo::ConstPtr &msg){ From 2e9c59c00c059058b00f352ede5f83bec445e2df Mon Sep 17 00:00:00 2001 From: Tai Hoang Date: Tue, 24 Aug 2021 15:20:42 +0200 Subject: [PATCH 06/10] Using controller.yaml --- include/kindyn/robot.hpp | 3 +-- src/robot.cpp | 47 +++++++++++++++++++++------------------ src/robots/upper_body.cpp | 11 ++++++++- 3 files changed, 36 insertions(+), 25 deletions(-) diff --git a/include/kindyn/robot.hpp b/include/kindyn/robot.hpp index bddbc4b..a98dbb0 100644 --- a/include/kindyn/robot.hpp +++ b/include/kindyn/robot.hpp @@ -135,6 +135,7 @@ namespace cardsflow { ros::Subscriber controller_type_sub, joint_state_sub, floating_base_sub, interactive_marker_sub, joint_target_sub, zero_joints_sub; /// ROS subscribers ros::ServiceServer ik_srv, ik_two_frames_srv, fk_srv, freeze_srv; string topic_root; + string robot_model_; VectorXd q, qd, qdd; /// joint positon, velocity, acceleration VectorXd q_target, qd_target, qdd_target; /// joint positon, velocity, acceleration targets @@ -149,9 +150,7 @@ namespace cardsflow { bool external_robot_state; /// indicates if we get the robot state externally bool external_robot_target = false; bool debug_ = false; - double k_dt = 0.005; VectorXd Kp_, Kd_; - vector param_kp, param_kd; }; }; } diff --git a/src/robot.cpp b/src/robot.cpp index 927de77..acd6534 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -99,20 +99,22 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vectorgetParam("joint_dt", kinematics.joint_dt); - nh->getParam("joint_kp", param_kp); - nh->getParam("joint_kd", param_kd); for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { - Kp_[joint] = param_kp[joint]; - Kd_[joint] = param_kd[joint]; - ROS_INFO_STREAM(kinematics.joint_names[joint] << "\tdt=" << kinematics.joint_dt[joint] << - "\tkp=" << param_kp[joint] << "\tkd=" << param_kd[joint]); + std::string control_param = robot_model_ + "_" + kinematics.joint_names[joint]; + nh->getParam(control_param + "/Kp", param_kp); + nh->getParam(control_param + "/Kd", param_kd); + nh->getParam(control_param + "/dt", param_dt); + + Kp_[joint] = param_kp; + Kd_[joint] = param_kd; + kinematics.joint_dt[joint] = param_dt; + ROS_INFO_STREAM(control_param << "\tdt=" << kinematics.joint_dt[joint] << + "\tkp=" << Kp_[joint] << "\tkd=" << Kd_[joint]); } for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { @@ -182,20 +184,21 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vectorhasParam("joint_dt")) - nh->getParam("joint_dt", kinematics.joint_dt); - if (nh->hasParam("joint_kp")){ - nh->getParam("joint_kp", param_kp); - for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { - Kp_[joint] = param_kp[joint]; - } - } - if (nh->hasParam("joint_kd")) { - nh->getParam("joint_kd", param_kd); - for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { - Kd_[joint] = param_kd[joint]; - } + double param_kp, param_kd, param_dt; + stringstream update_str; + update_str << "Updated controller params" << endl; + for (int joint = 0; joint < kinematics.number_of_dofs; joint++) { + std::string control_param = robot_model_ + "_" + kinematics.joint_names[joint]; + nh->getParam(control_param + "/Kp", param_kp); + nh->getParam(control_param + "/Kd", param_kd); + nh->getParam(control_param + "/dt", param_dt); + Kp_[joint] = param_kp; + Kd_[joint] = param_kd; + kinematics.joint_dt[joint] = param_dt; + update_str << control_param << "\tdt=" << kinematics.joint_dt[joint] + << "\tkp=" << Kp_[joint] << "\tkd=" << Kd_[joint] << endl; } + ROS_INFO_STREAM_THROTTLE(1, update_str.str()); } // TODO: Run the below code in critical section to avoid Mutex with the JointState and PROBABLY the controller diff --git a/src/robots/upper_body.cpp b/src/robots/upper_body.cpp index d2e409c..417ed26 100644 --- a/src/robots/upper_body.cpp +++ b/src/robots/upper_body.cpp @@ -1,6 +1,7 @@ // #include "kindyn/vrpuppet.hpp" #include "kindyn/robot.hpp" #include +#include #include #include #include @@ -56,6 +57,7 @@ class UpperBody: public cardsflow::kindyn::Robot{ } debug_ = debug; + robot_model_ = robot_model; nh = ros::NodeHandlePtr(new ros::NodeHandle); spinner = new ros::AsyncSpinner(0); @@ -522,7 +524,14 @@ void update(controller_manager::ControllerManager *cm) { int main(int argc, char *argv[]) { string robot_model(argv[1]); - bool debug(argv[2]); + bool debug; + + if(strcmp(argv[2], "true") == 0){ + debug = true; + }else{ + debug = false; + } + ROS_INFO_STREAM("launching " << robot_model); if (!ros::isInitialized()) { int argc = 0; From d14d00edecea3a81b4ba04789db8de85a711d824 Mon Sep 17 00:00:00 2001 From: Tai Hoang Date: Tue, 24 Aug 2021 15:54:20 +0200 Subject: [PATCH 07/10] Re-enable the regularization --- src/kinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/kinematics.cpp b/src/kinematics.cpp index b07696e..b09e74a 100644 --- a/src/kinematics.cpp +++ b/src/kinematics.cpp @@ -329,7 +329,7 @@ vector Kinematics::oneStepForward(VectorXd& q_in, VectorXd& qd_in, vec for(int i = 0; i Date: Tue, 24 Aug 2021 16:18:11 +0200 Subject: [PATCH 08/10] Remove control_parameters.yaml file in robot.launch --- launch/robot.launch | 1 - 1 file changed, 1 deletion(-) diff --git a/launch/robot.launch b/launch/robot.launch index fc43ada..61f2ee3 100644 --- a/launch/robot.launch +++ b/launch/robot.launch @@ -38,7 +38,6 @@ - [0.0, 0.0, 0.0] 5.0 5.0 From 0126defdd028570e18248d34e421fb75c0d32299 Mon Sep 17 00:00:00 2001 From: Alona Kharchenko Date: Wed, 8 Sep 2021 13:38:48 +0200 Subject: [PATCH 09/10] info print for debug --- src/robots/upper_body.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/robots/upper_body.cpp b/src/robots/upper_body.cpp index 417ed26..d4ce79e 100644 --- a/src/robots/upper_body.cpp +++ b/src/robots/upper_body.cpp @@ -528,6 +528,7 @@ int main(int argc, char *argv[]) { if(strcmp(argv[2], "true") == 0){ debug = true; + ROS_INFO_WARN_STREAM("DEBUG mode enabled"); }else{ debug = false; } From 47b9e982cca8a4357575ab9028265fb4499e3897 Mon Sep 17 00:00:00 2001 From: Alona Kharchenko Date: Wed, 8 Sep 2021 13:44:30 +0200 Subject: [PATCH 10/10] Update upper_body.cpp --- src/robots/upper_body.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/robots/upper_body.cpp b/src/robots/upper_body.cpp index d4ce79e..99a7957 100644 --- a/src/robots/upper_body.cpp +++ b/src/robots/upper_body.cpp @@ -528,7 +528,7 @@ int main(int argc, char *argv[]) { if(strcmp(argv[2], "true") == 0){ debug = true; - ROS_INFO_WARN_STREAM("DEBUG mode enabled"); + ROS_WARN_STREAM("DEBUG mode enabled"); }else{ debug = false; }