dyros_jet_controller/src/haptic_controller.cpp
// Jacobian OK. Translation OK.
Jacobian OK. Translation OK.
Jacobian OK . Translation OK .
void HapticController::computeCLIK()
{
    const double inverse_damping = 1e-4;
    const double phi_gain = 0.5;
    const double kp = 200;
    const double epsilon = 1e-3;
    for(unsigned int i=2; i<4; i++)
    {
        if(ee_enabled_[i])
        {
            const auto &x = model_.getCurrentTransform((DyrosJetModel::EndEffector)(i)).translation();
            const auto &rot = model_.getCurrentTransform((DyrosJetModel::EndEffector)(i)).linear();
            const auto &x_target = target_transform_[i].translation();
            Eigen::Vector6d x_dot_desired;
            Eigen::Vector6d x_error;
            x_error.head<3>() = (x_target - x);
            x_error.tail<3>() =  - phi_gain * DyrosMath::getPhi(rot, rot_init_);
            x_dot_desired.head<3>() = x_target - x_prev_[i];
            x_dot_desired.tail<3>().setZero();
            x_error = x_error; 
            x_dot_desired = x_dot_desired * hz_;
            x_prev_[i] = x_target;
            if (x_error.norm() < epsilon && control_time_ >= end_time_[i]) 
            {
                target_arrived_[i] = true;
                ee_enabled_[i] = false;
                ROS_INFO("target arrived - %d End effector", i);
                continue;
            }
            const auto &J = model_.getArmJacobian((DyrosJetModel::EndEffector)(i));
            const auto &q = current_q_.segment<7>(model_.joint_start_index_[i]);
            auto J_inverse = J.transpose() *
                    (inverse_damping * Eigen::Matrix6d::Identity() +
                     J * J.transpose()).inverse();
            desired_q_.segment<7>(model_.joint_start_index_[i]) =
                    (J_inverse * (x_dot_desired + x_error * kp)) / hz_ + q;
        }
    }
}
void HapticController :: computeCLIK ( ) { const double inverse_damping = 1e-4 ; const double phi_gain = 0.5 ; const double kp = 200 ; const double epsilon = 1e-3 ; for ( unsigned int i = 2 ; i < 4 ; i ++ ) { if ( ee_enabled_ [ i ] ) { const auto & x = model_ . getCurrentTransform ( ( DyrosJetModel :: EndEffector ) ( i ) ) . translation ( ) ; const auto & rot = model_ . getCurrentTransform ( ( DyrosJetModel :: EndEffector ) ( i ) ) . linear ( ) ; const auto & x_target = target_transform_ [ i ] . translation ( ) ; Eigen :: Vector6d x_dot_desired ; Eigen :: Vector6d x_error ; x_error . head < 3 > ( ) = ( x_target - x ) ; x_error . tail < 3 > ( ) = - phi_gain * DyrosMath :: getPhi ( rot , rot_init_ ) ; x_dot_desired . head < 3 > ( ) = x_target - x_prev_ [ i ] ; x_dot_desired . tail < 3 > ( ) . setZero ( ) ; x_error = x_error ; x_dot_desired = x_dot_desired * hz_ ; x_prev_ [ i ] = x_target ; if ( x_error . norm ( ) < epsilon && control_time_ >= end_time_ [ i ] ) { target_arrived_ [ i ] = true ; ee_enabled_ [ i ] = false ; ROS_INFO ( " " , i ) ; continue ; } const auto & J = model_ . getArmJacobian ( ( DyrosJetModel :: EndEffector ) ( i ) ) ; const auto & q = current_q_ . segment < 7 > ( model_ . joint_start_index_ [ i ] ) ; auto J_inverse = J . transpose ( ) * ( inverse_damping * Eigen :: Matrix6d :: Identity ( ) + J * J . transpose ( ) ) . inverse ( ) ; desired_q_ . segment < 7 > ( model_ . joint_start_index_ [ i ] ) = ( J_inverse * ( x_dot_desired + x_error * kp ) ) / hz_ + q ; } } }
Jacobian OK.
Jacobian OK .
// Arm // For short names //debug_ << control_time_ << "\t" << x(0) << "\t" << x(1) << "\t" <<x(2) << std::endl; //x_error.tail<3>().setZero(); //* hz_; // target arrived, cubic finished