VectorXd DampedNumericalFilteredController::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{
thetas_ = thetas;
///--Controller Step
//Calculate jacobian
task_jacobian_ = robot_.analyticalJacobian(thetas_);
// Recalculation of measured data.
// End effectors pose
end_effector_pose_ = robot_.fkm(thetas_);
//Error
last_error_ = error_;
error_ = vec8(reference - end_effector_pose_);
integral_error_ += error_;
//error_ = vec8(dq_one_ - conj(end_effector_pose_)*reference);
svd_.compute(task_jacobian_, ComputeFullU);
singular_values_ = svd_.singularValues();
//Damping Calculation
double sigma_min = singular_values_(5);
VectorXd u_min = svd_.matrixU().col(5);
double lambda = lambda_max_;
if (sigma_min < epsilon_)
{
lambda = (1-(sigma_min/epsilon_)*(sigma_min/epsilon_))*lambda_max_*lambda_max_;
}
//We want to solve the equation J+ = J^T(JJ^T+aI)^-1, in which the matrix
//being inverted is obviously positive definite if a > 0.
//The solver gives us the solution to X = A^-1.B
//Therefore I chose to find X^T = B^T(A^T)^-1 then take the transpose of X.
task_jacobian_pseudoinverse_ = ((task_jacobian_*task_jacobian_.transpose() + (beta_*beta_)*identity_ + (lambda*lambda)*u_min*u_min.transpose()).transpose()).ldlt().solve(task_jacobian_);
task_jacobian_pseudoinverse_.transposeInPlace();
if( at_least_one_error_ )
delta_thetas_ = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ + kd_*(error_ - last_error_) );
else
{
at_least_one_error_ = true;
delta_thetas_ = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ );
}
return delta_thetas_;
}
/** Computes the dynamics (task model)
* Assumes that the data_->model_.gc_model_ has been updated. */
bool CTaskOpPosition::computeModel(const SRobotSensors* arg_sensors)
{
#ifdef DEBUG
assert(has_been_init_);
assert(data_->has_been_init_);
assert(S_NULL!=data_->rbd_);
assert(S_NULL!=dynamics_);
#endif
if(data_->has_been_init_)
{
bool flag = true;
const SGcModel* gcm = data_->gc_model_;
flag = flag && dynamics_->computeJacobian(data_->J_6_,*(data_->rbd_),
arg_sensors->q_,data_->pos_in_parent_);
//Use the position jacobian only. This is an op-point task.
data_->J_ = data_->J_6_.block(0,0,3,data_->robot_->dof_);
//Operational space mass/KE matrix:
// // NOTE TODO : Decide a good scheme for disabling operational space inertia
// if(data_->flag_compute_op_inertia_)
// { }
// else
// { data_->M_task_inv_ = Eigen::Matrix3d::Identity(); }
//Lambda = (J * Ainv * J')^-1
data_->M_task_inv_ = data_->J_ * gcm->M_gc_inv_ * data_->J_.transpose();
#ifdef SCL_PRINT_INFO_MESSAGES
std::cout<<"\n\tJx6:\n"<<data_->J_6_
<<"\n\tFgrav_gc:\n"<<data_->gc_model_->force_gc_grav_.transpose();
std::cout<<"\n\tMx_inv:\n"<<data_->M_task_inv_
<<"\n\tJx:\n"<<data_->J_
<<"\n\tJx6:\n"<<data_->J_6_
<<"\n\tMgcinv:\n"<<gcm->M_gc_inv_;
std::cout<<"\n\tTo_lnk: \n"<<data_->rbd_->T_o_lnk_.matrix()
<<"\n\tPosInPar: "<<data_->pos_in_parent_.transpose()
<<"\n\t X: "<<data_->x_.transpose()
<<"\n\t Xgoal: "<<data_->x_goal_.transpose()
<<"\n\t Ftask: "<<data_->force_task_.transpose()
<<"\n\t Ftaskgc: "<<data_->force_gc_.transpose()
<<"\n\t Fxgrav: "<<data_->force_task_grav_.transpose();
#endif
if(!use_svd_for_lambda_inv_)
{
//The general inverse function works very well for op-point controllers.
//3x3 matrix inversion behaves quite well. Even near singularities where
//singular values go down to ~0.001. If the model is coarse, use a n-k rank
//approximation with the SVD for a k rank loss in a singularity.
qr_.compute(data_->M_task_inv_);
if(qr_.isInvertible())
{ data_->M_task_ = qr_.inverse(); }
else
{ use_svd_for_lambda_inv_ = true; }
}
if(use_svd_for_lambda_inv_)
{
//Use a Jacobi svd. No preconditioner is required coz lambda inv is square.
//NOTE : This is slower and generally performs worse than the simple inversion
//for small (3x3) matrices that are usually used in op-space controllers.
svd_.compute(data_->M_task_inv_,
Eigen::ComputeFullU | Eigen::ComputeFullV | Eigen::ColPivHouseholderQRPreconditioner);
#ifdef DEBUG
std::cout<<"\n Singular values : "<<svd_.singularValues().transpose();
#endif
int rank_loss=0;
//NOTE : A threshold of .005 works quite well for most robots.
//Experimentally determined: Take the robot to a singularity
//and observe the response as you allow the min singular values
//to decrease. Stop when the robot starts to go unstable.
//NOTE : This also strongly depends on how good your model is
//and how fast you update it. A bad model will require higher
//thresholds and will result in coarse motions. A better model
//will allow much lower thresholds and will result in smooth
//motions.
if(svd_.singularValues()(0) > 0.005)
{ singular_values_(0,0) = 1.0/svd_.singularValues()(0); }
else { singular_values_(0,0) = 0.0; rank_loss++; }
if(svd_.singularValues()(1) > 0.005)
{ singular_values_(1,1) = 1.0/svd_.singularValues()(1); }
else { singular_values_(1,1) = 0.0; rank_loss++; }
if(svd_.singularValues()(2) > 0.005)
{ singular_values_(2,2) = 1.0/svd_.singularValues()(2); }
else { singular_values_(2,2) = 0.0; rank_loss++; }
if(0 < rank_loss)
{ std::cout<<"\nCTaskOpPosition::computeModel() : Warning. Lambda_inv is ill conditioned. SVD rank loss (@.005) = "<<rank_loss; }
data_->M_task_ = svd_.matrixV() * singular_values_ * svd_.matrixU().transpose();
//Turn off the svd after 50 iterations
//Don't worry, the qr will pop back to svd if it is still singular
static sInt svd_ctr = 0; svd_ctr++;
if(50>=svd_ctr)
{ svd_ctr = 0; use_svd_for_lambda_inv_ = false; }
}
//Compute the Jacobian dynamically consistent generalized inverse :
//J_dyn_inv = Ainv * J' (J * Ainv * J')^-1
data_->J_dyn_inv_ = gcm->M_gc_inv_ * data_->J_.transpose() * data_->M_task_;
//J' * J_dyn_inv'
sUInt dof = data_->robot_->dof_;
data_->null_space_ = Eigen::MatrixXd::Identity(dof, dof) -
data_->J_.transpose() * data_->J_dyn_inv_.transpose();
// We do not use the centrifugal/coriolis forces. They can cause instabilities.
// NOTE TODO : Fix this...
// if(data_->flag_compute_op_gravity_)
// { /** I need some code */ }
// else
// { data_->force_task_cc_.setZero(data_->dof_task_,1); }
data_->force_task_cc_.setZero(data_->dof_task_,1);
// J' * J_dyn_inv' * g(q)
if(data_->flag_compute_op_gravity_)
{ data_->force_task_grav_ = data_->J_dyn_inv_.transpose() * gcm->force_gc_grav_; }
return flag;
}
else
{ return false; }
}
VectorXd Pid_SRIvar_TrackingController::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{
thetas_ = thetas; //This is necessary for the getNewJointPositions to work
DQ robot_base = robot_.base();
DQ robot_effector = robot_.effector();
VectorXd pseudo_dummy_joint_marker = VectorXd::Zero(robot_dofs_);
VectorXd pseudo_thetas = thetas_;
VectorXd pseudo_delta_thetas = VectorXd::Zero(robot_dofs_);
VectorXd possible_new_thetas = VectorXd::Zero(robot_dofs_);
MatrixXd original_dh_matrix = robot_.getDHMatrix();
MatrixXd step_dh_matrix = original_dh_matrix;
DQ_kinematics pseudo_robot(original_dh_matrix);
pseudo_robot.set_base(robot_base);
pseudo_robot.set_effector(robot_effector);
bool should_break_loop = false;
while(not should_break_loop) {
//Calculate jacobian
task_jacobian_ = pseudo_robot.analyticalJacobian(pseudo_thetas);
// Recalculation of measured data.
end_effector_pose_ = pseudo_robot.fkm(pseudo_thetas);
//Error
last_error_ = error_;
error_ = vec8(reference - end_effector_pose_);
integral_error_ = error_ + ki_memory_*integral_error_;
//error_ = vec8(dq_one_ - conj(end_effector_pose_)*reference);
///Inverse calculation
svd_.compute(task_jacobian_, ComputeFullU);
singular_values_ = svd_.singularValues();
//Damping Calculation
int current_step_relevant_dof = ( pseudo_robot.links() - pseudo_robot.n_dummy() );
double sigma_min = singular_values_( current_step_relevant_dof - 1);
VectorXd u_min = svd_.matrixU().col( current_step_relevant_dof - 1);
double lambda = lambda_max_;
if (sigma_min < epsilon_)
{
lambda = (1-(sigma_min/epsilon_)*(sigma_min/epsilon_))*lambda_max_*lambda_max_;
}
task_jacobian_pseudoinverse_ = (task_jacobian_.transpose())*((task_jacobian_*task_jacobian_.transpose()
+ (beta_*beta_)*identity_ + (lambda*lambda)*u_min*u_min.transpose()).inverse());
// pseudo_delta_thetas = task_jacobian_pseudoinverse_*kp_*error_;
if( at_least_one_error_ )
pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ + kd_*(error_ - last_error_) );
else
{
at_least_one_error_ = true;
pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ );
}
//Update delta_thetas for possiblenewthetas calculation
for(int i=0,j=0; i < robot_dofs_; i++)
{
if(pseudo_dummy_joint_marker(i) == 0)
{
delta_thetas_(i) = pseudo_delta_thetas(j);
++j;
}
//Do NOTHING if it should be ignored.
}
//Possible new thetas
possible_new_thetas = thetas_ + delta_thetas_;
//Verify if loop should end
should_break_loop = true;
int j=0;
//For all joints
for(int i = 0; i < robot_dofs_; i++) {
//If joint is not yet marked for not being considered in the minimization
if(pseudo_dummy_joint_marker(i) == 0) {
//If the controller is trying to put a joint further than any of its limits
if( possible_new_thetas(i) > upper_joint_limits_(i)
|| possible_new_thetas(i) < lower_joint_limits_(i) )
{
//If the joint was already saturated sometime ago
double ep = 1.e-05;
if ( thetas_(i) > upper_joint_limits_(i) - ep
|| thetas_(i) < lower_joint_limits_(i) + ep) {
pseudo_dummy_joint_marker(i) = 1; //Mark it to be ignored in the minization
//std::cout << std::endl << "Joint " << i << " will be ignored in the next controller step.";
step_dh_matrix(4,i) = 1; //Set matrix as dummy.
step_dh_matrix(0,i) = thetas_(i); //Set matrix theta as a fixed value.
should_break_loop = false;
}
//If the joint was not yet saturated and the controller wants to saturate it
else {
// Saturate the joint in this step.
if ( possible_new_thetas(i) > upper_joint_limits_(i) ) {
delta_thetas_(i) = upper_joint_limits_(i) - thetas_(i);
//std::cout << std::endl << "Joint = " << i << " was saturated in its upper_limit";
}
else if ( possible_new_thetas(i) < lower_joint_limits_(i) ) {
delta_thetas_(i) = lower_joint_limits_(i) - thetas_(i);
//std::cout << std::endl << "Joint = " << i << " was saturated in its lower_limit";
}
else {
std::cout << std::endl << "Something is really wrong";
}
//The joint should still be considered in the minimizations.
pseudo_thetas(j) = thetas_(i);
++j;
}
}
//If the controller is not trying to put this joint further than any of its limits, we consider the velocity given normally
else {
delta_thetas_(i) = pseudo_delta_thetas(j);
pseudo_thetas(j) = thetas_(i);
++j;
}
}
//If joint was marked to be ignored, it shall be ignored.
else {
delta_thetas_(i) = 0;
}
}
if( j == 0 ) {
std::cout << std::endl << "Robot will be unable to get out of this configuration using this controller.";
delta_thetas_ = VectorXd::Zero(robot_dofs_);
break;
}
if(not should_break_loop) {
pseudo_robot = DQ_kinematics(step_dh_matrix); //Change DH
pseudo_robot.set_base(robot_base);
pseudo_robot.set_effector(robot_effector);
pseudo_thetas.conservativeResize(j); //Resize pseudothetas
}
}//While not should_break_loop
return delta_thetas_;
}