Refactor kindyn

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

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

include/kindyn/kinematics.hpp

@@ -59,7 +59,7 @@ namespace cardsflow {
             /**
              * Do forward kinematics
              */
-            vector<VectorXd> oneStepForward(double dt, VectorXd& q_in, VectorXd& qd_in, vector<VectorXd> Ld);
+            vector<VectorXd> oneStepForward(VectorXd& q_in, VectorXd& qd_in, vector<VectorXd> 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<string> link_relation_name;
+            vector<vector<string>> joint_relation_name;
+            vector<double> joint_dt;
+
             vector<Cable> cables; /// all cables of the robot
             vector <VectorXd> joint_axis; /// joint axis of each joint
             vector <string> link_names, joint_names; /// link and joint names of the robot

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_;
         };
     };
 }

launch/robot.launch

@@ -37,16 +37,14 @@
     <rosparam file="$(find robots)/$(arg model_name)/config/controller.yaml" command="load"/>
     <rosparam file="$(find robots)/$(arg model_name)/config/endeffectors.yaml" command="load"/>
     <rosparam file="$(find robots)/$(arg model_name)/config/motor_config.yaml" command="load"/>
+    <rosparam file="$(find robots)/$(arg model_name)/config/link_joint_relation.yaml" command="load"/>
     <rosparam param="q_target">[0.0, 0.0, 0.0]</rosparam>
     <rosparam param="pwm">5.0</rosparam>
     <rosparam param="m3_pwm">5.0</rosparam>
     <param name="min_force" type="double" value="0"  />
     <param name="max_force" type="double" value="100000"  />
     <param name="controller" type="int" value="2"  />
     <param name="external_robot_state" type="bool" value="$(arg external_robot_state)"/>
-    <param name="k_dt" type="double" value="0.005"  />
-    <param name="Kp" type="double" value="100"  />
-    <param name="Kd" type="double" value="0"  />
 
     <node name="$(arg robot_model)_controller_manager"
           pkg="controller_manager"

src/controller/cableLengthController.cpp

@@ -91,9 +91,12 @@ class CableLengthController : public controller_interface::Controller<hardware_i
         double q = joint.getPosition();
         double q_target = joint.getJointPositionCommand();
         MatrixXd L = joint.getL();
+        VectorXd Kp = *joint.Kp_;
+        VectorXd Kd = *joint.Kd_;
+
         double p_error = wrap_pos_neg_pi(q - q_target);
         // we use the joint_index column of the L matrix to calculate the result for this joint only
-        VectorXd ld = L.col(joint_index) * ((*joint.Kd_) * (p_error - p_error_last)/period.toSec() + (*joint.Kp_) * p_error);
+        VectorXd ld = L.col(joint_index) * (Kd[joint_index] * (p_error - p_error_last)/period.toSec() + Kp[joint_index] * p_error);
         joint.setMotorCommand(ld);
         p_error_last = p_error;
         last_update = time;

src/controller/cardsflow_command_interface.cpp

@@ -13,7 +13,7 @@ namespace hardware_interface
      * @param Kd A pointer to D gain
      */
     CardsflowHandle::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_velocity_cmd, double* joint_torque_cmd, VectorXd *motor_cmd, VectorXd *Kp, VectorXd *Kd)
             : CardsflowStateHandle(js), joint_position_cmd_(joint_position_cmd), joint_velocity_cmd_(joint_velocity_cmd),
               joint_torque_cmd_(joint_torque_cmd), motor_cmd_(motor_cmd), Kp_(Kp), Kd_(Kd)
     {

src/kinematics.cpp

@@ -228,20 +228,7 @@ 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();
-                }
-
-                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<VectorXd> Kinematics::oneStepForward(double dt, VectorXd& q_in, VectorXd& qd_in, vector<VectorXd> Ld) {
+vector<VectorXd> Kinematics::oneStepForward(VectorXd& q_in, VectorXd& qd_in, vector<VectorXd> Ld) {
+
+    integration_time = ros::Time::now().toSec();
 
     for(int i=0;i<number_of_joints;i++){
         joint_state[i][0] = q_in[i];
@@ -335,7 +338,7 @@ vector<VectorXd> 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<VectorXd> Kinematics::oneStepForward(double dt, VectorXd& q_in, VectorXd&
         }
     }
 
-    integration_time += dt;
     ROS_INFO_THROTTLE(10, "forward kinematics calculated for %lf s", integration_time);
 
     vector<VectorXd> result = {q_next, qd_next};

src/robot.cpp

@@ -51,7 +51,14 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vector<strin
     fmt = Eigen::IOFormat(4, 0, " ", ";\n", "", "", "[", "]");
     nh->getParam("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<string> 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, vector<strin
      * Register ROS control
      */
     // PD gains for cable length controller
-    Kp_ = new double(100.0);
-    Kd_ = new double(0.0);
-    if (nh->hasParam("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, vector<strin
         // connect and register the cardsflow command interface
         hardware_interface::CardsflowHandle pos_handle(cardsflow_state_interface.getHandle(kinematics.joint_names[joint]),
                                                        &q_target[joint], &qd_target[joint], &kinematics.torques[joint], &ld[joint],
-                                                       Kp_, Kd_);
+                                                       &Kp_, &Kd_);
         cardsflow_command_interface.registerHandle(pos_handle);
     }
     registerInterface(&cardsflow_command_interface);
@@ -167,12 +184,21 @@ void Robot::init(string urdf_file_path, string viapoints_file_path, vector<strin
 void Robot::update(){
 
     if (debug_) {
-        if (nh->hasParam("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<VectorXd> state_next = kinematics.oneStepForward(k_dt, q, qd, Ld);
+    vector<VectorXd> state_next = kinematics.oneStepForward(q, qd, Ld);
 
     if(!this->external_robot_state){
         for(int i=0;i<kinematics.number_of_joints;i++){