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/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 422bd1a..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); @@ -71,6 +71,10 @@ 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 joint_dt; + 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/include/kindyn/robot.hpp b/include/kindyn/robot.hpp index 3fcd4a1..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,8 +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; - double *Kp_, *Kd_; + VectorXd Kp_, Kd_; }; }; } diff --git a/launch/robot.launch b/launch/robot.launch index 2493fbc..61f2ee3 100644 --- a/launch/robot.launch +++ b/launch/robot.launch @@ -37,6 +37,7 @@ + [0.0, 0.0, 0.0] 5.0 5.0 @@ -44,9 +45,6 @@ - - - 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(); - } - - 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,12 +249,23 @@ 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++; + + int k = 0; + 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_relation_name[i]){ + link_idx = l; + break; + } + } + + for (int j=0; j < joint_relation_name[i].size(); j++){ + S.block(6 * link_idx, k, 6, 1) = joint_axis[k]; + k++; + } } -// ROS_INFO_STREAM("S_t = " << S.transpose().format(fmt)); } void Kinematics::update_P() { @@ -282,9 +280,12 @@ 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(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); + + 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 +298,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; @@ -316,7 +317,9 @@ void Kinematics::update_P() { } -vector 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]; } @@ -353,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/robot.cpp b/src/robot.cpp index bd7868a..acd6534 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); @@ -91,14 +98,24 @@ 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_); + + double param_kp, param_kd, param_dt; + kinematics.joint_dt.resize(kinematics.number_of_dofs); + Kp_.resize(kinematics.number_of_dofs); + Kd_.resize(kinematics.number_of_dofs); + + 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; + 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++) { ROS_INFO("initializing controllers for joint %d %s", joint, kinematics.joint_names[joint].c_str()); @@ -112,7 +129,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_); + 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 @@ -210,7 +236,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 +#include #include #include #include @@ -40,7 +41,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 @@ -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); @@ -272,7 +274,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 +283,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 +300,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 +322,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); } @@ -359,6 +361,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 +378,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){ @@ -520,7 +524,15 @@ 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; + ROS_WARN_STREAM("DEBUG mode enabled"); + }else{ + debug = false; + } + ROS_INFO_STREAM("launching " << robot_model); if (!ros::isInitialized()) { int argc = 0;