int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac)
{
if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns())
return -1;
T_tmp = Frame::Identity();
SetToZero(t_tmp);
int j=0;
int k=0;
Frame total;
for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
//Calculate new Frame_base_ee
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//pose of the new end-point expressed in the base
total = T_tmp*chain.getSegment(i).pose(q_in(j));
//changing base of new segment's twist to base frame if it is not locked
//t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
if(!locked_joints_[j])
t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
}else{
total = T_tmp*chain.getSegment(i).pose(0.0);
}
//Changing Refpoint of all columns to new ee
changeRefPoint(jac,total.p-T_tmp.p,jac);
//Only increase jointnr if the segment has a joint
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//Only put the twist inside if it is not locked
if(!locked_joints_[j])
jac.setColumn(k++,t_tmp);
j++;
}
T_tmp = total;
}
return 0;
}
/*---------------------------------------------------------------
arkMassLapackBandSetup does the setup operations for the band
mass matrix solver. It constructs the mass matrix M, updates
counters, and calls the band LU factorization routine.
---------------------------------------------------------------*/
static int arkMassLapackBandSetup(ARKodeMem ark_mem, N_Vector tmp1,
N_Vector tmp2, N_Vector tmp3)
{
ARKDlsMassMem arkdls_mem;
int ier, retval;
int intn, iml, imu, ldmat;
arkdls_mem = (ARKDlsMassMem) ark_mem->ark_mass_mem;
intn = (int) arkdls_mem->d_n;
iml = (int) arkdls_mem->d_ml;
imu = (int) arkdls_mem->d_mu;
ldmat = arkdls_mem->d_M->ldim;
SetToZero(arkdls_mem->d_M);
retval = arkdls_mem->d_bmass(arkdls_mem->d_n, arkdls_mem->d_mu,
arkdls_mem->d_ml, ark_mem->ark_tn,
arkdls_mem->d_M, arkdls_mem->d_M_data,
tmp1, tmp2, tmp3);
arkdls_mem->d_nme++;
if (retval < 0) {
arkProcessError(ark_mem, ARKDLS_MASSFUNC_UNRECVR, "ARKLAPACK",
"arkMassLapackBandSetup", MSGD_MASSFUNC_FAILED);
arkdls_mem->d_last_flag = ARKDLS_MASSFUNC_UNRECVR;
return(-1);
} else if (retval > 0) {
arkdls_mem->d_last_flag = ARKDLS_MASSFUNC_RECVR;
return(1);
}
/* Do LU factorization of M */
dgbtrf_f77(&intn, &intn, &iml, &imu, arkdls_mem->d_M->data,
&ldmat, arkdls_mem->d_pivots, &ier);
/* Return 0 if the LU was complete; otherwise return 1 */
arkdls_mem->d_last_flag = (long int) ier;
if (ier > 0) return(1);
return(0);
}
static int IDABandSetup(IDAMem IDA_mem, N_Vector yyp, N_Vector ypp,
N_Vector rrp, N_Vector tmp1, N_Vector tmp2,
N_Vector tmp3)
{
int retval;
long int retfac;
IDADlsMem idadls_mem;
idadls_mem = (IDADlsMem) lmem;
/* Increment nje counter. */
nje++;
/* Zero out JJ; call Jacobian routine jac; return if it failed. */
SetToZero(JJ);
retval = bjac(neq, mu, ml, tn, cj, yyp, ypp, rrp,
JJ, jacdata, tmp1, tmp2, tmp3);
if (retval < 0) {
IDAProcessError(IDA_mem, IDADLS_JACFUNC_UNRECVR, "IDASBAND", "IDABandSetup", MSGD_JACFUNC_FAILED);
last_flag = IDADLS_JACFUNC_UNRECVR;
return(-1);
}
if (retval > 0) {
last_flag = IDADLS_JACFUNC_RECVR;
return(+1);
}
/* Do LU factorization of JJ; return success or fail flag. */
retfac = BandGBTRF(JJ, pivots);
if (retfac != 0) {
last_flag = retfac;
return(+1);
}
last_flag = IDADLS_SUCCESS;
return(0);
}
/*
* idaLapackBandSetup does the setup operations for the band linear solver.
* It calls the Jacobian function to obtain the Newton matrix M = F_y + c_j*F_y',
* updates counters, and calls the band LU factorization routine.
*/
static int idaLapackBandSetup(IDAMem IDA_mem,
N_Vector yP, N_Vector ypP, N_Vector fctP,
N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
IDADlsMem idadls_mem;
int ier, retval;
int intn, iml, imu, ldmat;
idadls_mem = (IDADlsMem) lmem;
intn = (int) n;
iml = (int) ml;
imu = (int) mu;
ldmat = JJ->ldim;
/* Call Jacobian function */
nje++;
SetToZero(JJ);
retval = bjac(n, mu, ml, tn, cj, yP, ypP, fctP, JJ, J_data, tmp1, tmp2, tmp3);
if (retval < 0) {
IDAProcessError(IDA_mem, IDADLS_JACFUNC_UNRECVR, "IDALAPACK", "idaLapackBandSetup", MSGD_JACFUNC_FAILED);
last_flag = IDADLS_JACFUNC_UNRECVR;
return(-1);
} else if (retval > 0) {
last_flag = IDADLS_JACFUNC_RECVR;
return(+1);
}
/* Do LU factorization of M */
dgbtrf_f77(&intn, &intn, &iml, &imu, JJ->data, &ldmat, pivots, &ier);
/* Return 0 if the LU was complete; otherwise return 1 */
last_flag = (long int) ier;
if (ier > 0) return(1);
return(0);
}
static int kinLapackBandSetup(KINMem kin_mem)
{
KINDlsMem kindls_mem;
int ier, retval;
kindls_mem = (KINDlsMem) lmem;
nje++;
SetToZero(J);
retval = bjac(n, mu, ml, uu, fval, J, J_data, vtemp1, vtemp2);
if (retval != 0) {
last_flag = -1;
return(-1);
}
/* Do LU factorization of J */
dgbtrf_f77(&n, &n, &ml, &mu, J->data, &(J->ldim), pivots, &ier);
/* Return 0 if the LU was complete; otherwise return -1 */
last_flag = ier;
if (ier > 0) return(-1);
return(0);
}
CAMLprim value c_densematrix_set_to_zero(value va)
{
CAMLparam1(va);
SetToZero(DLSMAT(va));
CAMLreturn (Val_unit);
}
int main()
{
std::string icub_urdf_filename = "icub_model.urdf";
// Create the iCub model
KDL::Tree icub_tree;
bool ok = iDynTree::treeFromUrdfFile(icub_urdf_filename,icub_tree);
if( !ok )
{
std::cerr <<"Could not import icub urdf" << std::endl;
return EXIT_FAILURE;
}
// Create the iCub undirected tree, that contains also
// a default serialization (i.e. a mapping between links/joints
// and integers (if you want to provide a user-defined serialization
// to the undirected tree, pass it as a second argument to the constructor
KDL::CoDyCo::UndirectedTree icub_undirected_tree(icub_tree);
std::cout << "icub_tree serialization 1 : " << icub_undirected_tree.getSerialization().toString();
// Load a sensors tree (for ft sensors) from the information extracted from urdf file
// and using the serialization provided in the undirected tree
iDynTree::SensorsList sensors_tree = iDynTree::sensorsListFromURDF(icub_undirected_tree,icub_urdf_filename);
//Create a regressor generator
KDL::CoDyCo::Regressors::DynamicRegressorGenerator regressor_generator(icub_undirected_tree,sensors_tree);
//Add a set of rows of the regressor of right arm
std::vector<std::string> l_arm_subtree;
l_arm_subtree.push_back("l_upper_arm");
regressor_generator.addSubtreeRegressorRows(l_arm_subtree);
//Create a random state for the robot
KDL::Twist base_velocity, base_acceleration;
base_velocity = base_acceleration = KDL::Twist::Zero();
KDL::JntArray q(regressor_generator.getNrOfDOFs());
SetToZero(q);
srand(time(0));
for(int i=0; i < q.rows(); i++ )
{
q(i) = random_double();
}
double gravity_norm = 9.8;
base_acceleration.vel[2] = -gravity_norm;
regressor_generator.setRobotState(q,q,q,base_velocity,base_acceleration);
// Estimate sensor measurements from the model
iDynTree::Wrench simulate_measurement = simulateFTSensorFromKinematicState(icub_undirected_tree,
q,q,q,base_velocity,base_acceleration,"l_arm_ft_sensor",sensors_tree);
//Create a regressor and check the returned sensor value
Eigen::MatrixXd regressor(regressor_generator.getNrOfOutputs(),regressor_generator.getNrOfParameters());
Eigen::VectorXd kt(regressor_generator.getNrOfOutputs());
regressor_generator.computeRegressor(regressor,kt);
//std::cout << "regressors : " << regressor << std::endl;
Eigen::VectorXd parameters(regressor_generator.getNrOfParameters());
parameters.setZero();
regressor_generator.getModelParameters(parameters);
assert(parameters.rows() == regressor_generator.getNrOfParameters());
Eigen::Matrix<double,6,1> sens = regressor*parameters;
////////////////////////////////////////////////////////////
///// Test also the new iDynTree regressor infrastructure
////////////////////////////////////////////////////////////
iDynTree::Regressors::DynamicsRegressorGenerator new_generator;
// load robot and sensor model
ok = ok && new_generator.loadRobotAndSensorsModelFromFile(icub_urdf_filename);
assert(ok);
// load regressor structure (this should be actually loaded from file)
std::string regressor_structure
= "<regressor> "
" <subtreeBaseDynamics> "
" <FTSensorLink>l_upper_arm</FTSensorLink> "
" </subtreeBaseDynamics> "
"</regressor>";
ok = ok && new_generator.loadRegressorStructureFromString(regressor_structure);
assert(ok);
iDynTree::VectorDynSize q_idyntree(q.rows());
ok = ok && iDynTree::ToiDynTree(q,q_idyntree);
assert(ok);
iDynTree::Twist gravity;
gravity(2) = gravity_norm;
ok = ok && new_generator.setRobotState(q_idyntree,q_idyntree,q_idyntree,gravity);
assert(ok);
iDynTree::MatrixDynSize regr_idyntree(new_generator.getNrOfOutputs(),new_generator.getNrOfParameters());
iDynTree::VectorDynSize kt_idyntree(new_generator.getNrOfOutputs());
iDynTree::VectorDynSize param_idyntree(new_generator.getNrOfParameters());
ok = ok && new_generator.getModelParameters(param_idyntree);
int sensorIndex = new_generator.getSensorsModel().getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,"l_arm_ft_sensor");
ok = ok && new_generator.getSensorsMeasurements().setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,sensorIndex,simulate_measurement);
ok = ok && new_generator.computeRegressor(regr_idyntree,kt_idyntree);
Eigen::Matrix<double,6,1> sens_idyntree = Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(regr_idyntree.data(),regr_idyntree.rows(),regr_idyntree.cols())*
Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size());
Eigen::Matrix<double,6,1> kt_eigen = Eigen::Map< Eigen::Matrix<double,6,1> >(kt_idyntree.data(),kt_idyntree.size());
std::cout << "Parameters norm old " << parameters.norm() << std::endl;
std::cout << "Parameters norm new " << Eigen::Map<Eigen::VectorXd>(param_idyntree.data(),param_idyntree.size()).norm() << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters: " << sens << std::endl;
std::cout << "Sensor measurement from regressor*model_parameters (new): " << sens_idyntree << std::endl;
std::cout << "Sensor measurement from RNEA: " << KDL::CoDyCo::toEigen( iDynTree::ToKDL(simulate_measurement)) << std::endl;
std::cout << "Sensor measurement from known terms (new) " << kt_eigen << std::endl;
double tol = 1e-5;
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+sens_idyntree).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
if( (KDL::CoDyCo::toEigen(iDynTree::ToKDL(simulate_measurement))+kt_eigen).norm() > tol )
{
std::cerr << "[ERR] iCubLeftArmRegressor error: failure in new iDynTree regressor generator" << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
void SetToZero(Jacobian& jac)
{
for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++)
SetToZero(jac.twists[i]);
}
/*
* cvLapackBandSetup does the setup operations for the band linear solver.
* It makes a decision whether or not to call the Jacobian evaluation
* routine based on various state variables, and if not it uses the
* saved copy. In any case, it constructs the Newton matrix M = I - gamma*J,
* updates counters, and calls the band LU factorization routine.
*/
static int cvLapackBandSetup(CVodeMem cv_mem, int convfail,
N_Vector yP, N_Vector fctP,
booleantype *jcurPtr,
N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
{
CVDlsMem cvdls_mem;
realtype dgamma, fact;
booleantype jbad, jok;
int ier, retval, one = 1;
cvdls_mem = (CVDlsMem) lmem;
/* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
dgamma = ABS((gamma/gammap) - ONE);
jbad = (nst == 0) || (nst > nstlj + CVD_MSBJ) ||
((convfail == CV_FAIL_BAD_J) && (dgamma < CVD_DGMAX)) ||
(convfail == CV_FAIL_OTHER);
jok = !jbad;
if (jok) {
/* If jok = TRUE, use saved copy of J */
*jcurPtr = FALSE;
dcopy_f77(&(savedJ->ldata), savedJ->data, &one, M->data, &one);
} else {
/* If jok = FALSE, call jac routine for new J value */
nje++;
nstlj = nst;
*jcurPtr = TRUE;
SetToZero(M);
retval = bjac(n, mu, ml, tn, yP, fctP, M, J_data, tmp1, tmp2, tmp3);
if (retval == 0) {
dcopy_f77(&(M->ldata), M->data, &one, savedJ->data, &one);
} else if (retval < 0) {
cvProcessError(cv_mem, CVDLS_JACFUNC_UNRECVR, "CVSLAPACK", "cvLapackBandSetup", MSGD_JACFUNC_FAILED);
last_flag = CVDLS_JACFUNC_UNRECVR;
return(-1);
} else if (retval > 0) {
last_flag = CVDLS_JACFUNC_RECVR;
return(1);
}
}
/* Scale J by - gamma */
fact = -gamma;
dscal_f77(&(M->ldata), &fact, M->data, &one);
/* Add identity to get M = I - gamma*J*/
AddIdentity(M);
/* Do LU factorization of M */
dgbtrf_f77(&n, &n, &ml, &mu, M->data, &(M->ldim), pivots, &ier);
/* Return 0 if the LU was complete; otherwise return 1 */
last_flag = ier;
if (ier > 0) return(1);
return(0);
}
double TreeIkSolverPos_Online::CartToJnt_it(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
assert(q_out.rows() == q_in.rows());
assert(q_out_old_.rows() == q_in.rows());
assert(q_dot_.rows() == q_in.rows());
assert(q_dot_old_.rows() == q_in.rows());
assert(q_dot_new_.rows() == q_in.rows());
q_out = q_in;
SetToZero(q_dot_);
SetToZero(q_dot_old_);
SetToZero(q_dot_new_);
twist_ = Twist::Zero();
// First check, if all elements in p_in are available
unsigned int nr_of_endeffectors = 0;
nr_of_still_endeffectors_ = 0;
low_pass_adj_factor_ = 0.0;
for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
{
if(frames_.find(f_des_it->first)==frames_.end())
return -2;
else
{
/*
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
old_twists_it->second = Twist::Zero();
Frames::iterator f_0_it = frames_0_.find(f_des_it->first);
*/
/*
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
old_twists_it->second = diff(f_old_it->second, f_des_it->second);
if (){};
*/
Frames::iterator f_old_it = p_in_old_.find(f_des_it->first);
Frames::iterator f_des_pos_l_it = frames_pos_lim_.find(f_des_it->first);
twist_ = diff(f_old_it->second, f_des_it->second);
/*
if(sqrt(pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2)) < x_dot_trans_min_)
{
f_des_pos_l_it->second.p = addDelta(f_old_it->second.p, (0.1 * x_dot_trans_max_ * twist_.vel));
std::cout << "old position is used." << std::endl;
}
else
f_des_pos_l_it->second.p = f_des_it->second.p;
if(sqrt(pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2)) < x_dot_rot_min_)
{
f_des_pos_l_it->second.M = addDelta(f_old_it->second.M, (0.1 * x_dot_rot_max_ * twist_.rot));
std::cout << "old orientation is used." << std::endl;
}
else
f_des_pos_l_it->second.M = f_des_it->second.M;
*/
f_des_pos_l_it->second.p = f_des_it->second.p;
f_des_pos_l_it->second.M = f_des_it->second.M;
Frames::iterator f_des_vel_l_it = frames_vel_lim_.find(f_des_it->first);
fksolver_.JntToCart(q_in, f_des_vel_l_it->second, f_des_it->first);
twist_ = diff(f_des_vel_l_it->second, f_des_pos_l_it->second);
f_des_vel_l_it->second = addDelta(f_des_vel_l_it->second, twist_);
nr_of_endeffectors++;
}
}
if(nr_of_still_endeffectors_ == nr_of_endeffectors)
{
small_task_space_movement_ = true;
}
else
small_task_space_movement_ = false;
unsigned int k=0;
double res = 0.0;
while(++k <= maxiter_)
{
//for (Frames::const_iterator f_des_it=p_in.begin(); f_des_it!=p_in.end(); ++f_des_it)
for (Frames::const_iterator f_des_it=frames_vel_lim_.begin(); f_des_it!=frames_vel_lim_.end(); ++f_des_it)
{
// Get all iterators for this endpoint
Frames::iterator f_it = frames_.find(f_des_it->first);
Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
Frames::iterator f_0_it = frames_vel_lim_.find(f_des_it->first);
fksolver_.JntToCart(q_out, f_it->second, f_it->first);
// Checks, if the current overall twist exceeds the maximum translational and/or rotational velocity.
// If so, the velocities of the overall twist get scaled and a new current twist is calculated.
delta_twists_it->second = diff(f_it->second, f_des_it->second);
old_twists_it->second = diff(f_0_it->second, f_it->second);
enforceCartVelLimits_it(old_twists_it->second, delta_twists_it->second);
}
res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
if (res < eps_)
{
break;
//return res;
}
// Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
//Subtract(q_out, q_in, q_dot_old_);
//enforceJointVelLimits_it(q_dot_old_, q_dot_);
Subtract(q_out, q_in, q_dot_old_);
Add(q_dot_old_, q_dot_, q_dot_);
enforceJointVelLimits();
Subtract(q_dot_, q_dot_old_, q_dot_);
// Integrate
Add(q_out, q_dot_, q_out);
// Limit joint positions
for (unsigned int j = 0; j < q_min_.rows(); ++j)
{
if (q_out(j) < q_min_(j))
q_out(j) = q_min_(j);
else if (q_out(j) > q_max_(j))
q_out(j) = q_max_(j);
}
}
Subtract(q_out, q_in, q_dot_);
Add(q_in, q_dot_, q_out);
filter(q_dot_, q_out, q_out_old_);
q_out_old_ = q_out;
p_in_old_ = p_in;
if (k <= maxiter_)
return res;
else
return -3;
}
void MultiTaskPriorityInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
{
// get joint positions
for(int i=0; i < joint_handles_.size(); i++)
{
joint_msr_states_.q(i) = joint_handles_[i].getPosition();
joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
}
// clearing error msg before publishing
msg_err_.data.clear();
if (cmd_flag_)
{
// resetting P and qdot(t=0) for the highest priority task
P_ = I_;
SetToZero(joint_des_states_.qdot);
for (int index = 0; index < ntasks_; index++)
{
// computing Jacobian
jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_,links_index_[index]);
// computing forward kinematics
fk_pos_solver_->JntToCart(joint_msr_states_.q,x_,links_index_[index]);
// setting marker parameters
set_marker(x_,index,msg_id_);
// computing end-effector position/orientation error w.r.t. desired frame
x_err_ = diff(x_,x_des_[index]);
for(int i = 0; i < e_dot_.size(); i++)
{
e_dot_(i) = x_err_(i);
msg_err_.data.push_back(e_dot_(i));
}
// computing (J[i]*P[i-1])^pinv
J_star_.data = J_.data*P_;
pseudo_inverse(J_star_.data,J_pinv_);
// computing q_dot (qdot(i) = qdot[i-1] + (J[i]*P[i-1])^pinv*(x_err[i] - J[i]*qdot[i-1]))
joint_des_states_.qdot.data = joint_des_states_.qdot.data + J_pinv_*(e_dot_ - J_.data*joint_des_states_.qdot.data);
// stop condition
if (!on_target_flag_[index])
{
if (Equal(x_,x_des_[index],0.01))
{
ROS_INFO("Task %d on target",index);
on_target_flag_[index] = true;
if (index == (ntasks_ - 1))
cmd_flag_ = 0;
}
}
// updating P_ (it mustn't make use of the damped pseudo inverse)
pseudo_inverse(J_star_.data,J_pinv_,false);
P_ = P_ - J_pinv_*J_star_.data;
}
// integrating q_dot -> getting q (Euler method)
for (int i = 0; i < joint_handles_.size(); i++)
joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);
}
// set controls for joints
for (int i = 0; i < joint_handles_.size(); i++)
{
tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),joint_des_states_.qdot(i) - joint_msr_states_.qdot(i),period);
joint_handles_[i].setCommand(tau_cmd_(i));
}
// publishing markers for visualization in rviz
pub_marker_.publish(msg_marker_);
msg_id_++;
// publishing error for all tasks as an array of ntasks*6
pub_error_.publish(msg_err_);
ros::spinOnce();
}
void OneTaskInverseDynamicsJL::update(const ros::Time& time, const ros::Duration& period)
{
// get joint positions
for(int i=0; i < joint_handles_.size(); i++)
{
joint_msr_states_.q(i) = joint_handles_[i].getPosition();
joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
}
// clearing msgs before publishing
msg_err_.data.clear();
msg_pose_.data.clear();
if (cmd_flag_)
{
// resetting N and tau(t=0) for the highest priority task
N_trans_ = I_;
SetToZero(tau_);
// computing Inertia, Coriolis and Gravity matrices
id_solver_->JntToMass(joint_msr_states_.q, M_);
id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_);
id_solver_->JntToGravity(joint_msr_states_.q, G_);
G_.data.setZero();
// computing the inverse of M_ now, since it will be used often
pseudo_inverse(M_.data,M_inv_,false); //M_inv_ = M_.data.inverse();
// computing Jacobian J(q)
jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);
// computing the distance from the mid points of the joint ranges as objective function to be minimized
phi_ = task_objective_function(joint_msr_states_.q);
// using the first step to compute jacobian of the tasks
if (first_step_)
{
J_last_ = J_;
phi_last_ = phi_;
first_step_ = 0;
return;
}
// computing the derivative of Jacobian J_dot(q) through numerical differentiation
J_dot_.data = (J_.data - J_last_.data)/period.toSec();
// computing forward kinematics
fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);
if (Equal(x_,x_des_,0.05))
{
ROS_INFO("On target");
cmd_flag_ = 0;
return;
}
// pushing x to the pose msg
for (int i = 0; i < 3; i++)
msg_pose_.data.push_back(x_.p(i));
// setting marker parameters
set_marker(x_,msg_id_);
// computing end-effector position/orientation error w.r.t. desired frame
x_err_ = diff(x_,x_des_);
x_dot_ = J_.data*joint_msr_states_.qdot.data;
// setting error reference
for(int i = 0; i < e_ref_.size(); i++)
{
// e = x_des_dotdot + Kd*(x_des_dot - x_dot) + Kp*(x_des - x)
e_ref_(i) = -Kd_(i)*(x_dot_(i)) + Kp_(i)*x_err_(i);
msg_err_.data.push_back(e_ref_(i));
}
// computing b = J*M^-1*(c+g) - J_dot*q_dot
b_ = J_.data*M_inv_*(C_.data + G_.data) - J_dot_.data*joint_msr_states_.qdot.data;
// computing omega = J*M^-1*N^T*J
omega_ = J_.data*M_inv_*N_trans_*J_.data.transpose();
// computing lambda = omega^-1
pseudo_inverse(omega_,lambda_);
//lambda_ = omega_.inverse();
// computing nullspace
N_trans_ = N_trans_ - J_.data.transpose()*lambda_*J_.data*M_inv_;
// finally, computing the torque tau
tau_.data = J_.data.transpose()*lambda_*(e_ref_ + b_) + N_trans_*(Eigen::Matrix<double,7,1>::Identity(7,1)*(phi_ - phi_last_)/(period.toSec()));
// saving J_ and phi of the last iteration
J_last_ = J_;
phi_last_ = phi_;
}
// set controls for joints
for (int i = 0; i < joint_handles_.size(); i++)
{
if(cmd_flag_)
joint_handles_[i].setCommand(tau_(i));
else
joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period));
}
// publishing markers for visualization in rviz
pub_marker_.publish(msg_marker_);
msg_id_++;
// publishing error
pub_error_.publish(msg_err_);
// publishing pose
pub_pose_.publish(msg_pose_);
ros::spinOnce();
}
void SetToZero(JntArrayVel& array)
{
SetToZero(array.q);
SetToZero(array.qdot);
}
void Mtx44::SetToIdentity(void) {
SetToZero();
a[0] = a[5] = a[10] = a[15] = 1;
}
TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts,
const std::vector<std::string>& endpoints,
TreeFkSolverPos& fksolver,
TreeIkSolverVel& iksolver,
const JntArray& q_min,
const JntArray& q_max,
const JntArray& q_dot_min,
const JntArray& q_dot_max,
const double x_dot_trans_max,
const double x_dot_rot_max,
const double x_dot_trans_min,
const double x_dot_rot_min,
const double low_pass_factor,
const unsigned int maxiter,
const double eps) :
fksolver_(fksolver),
iksolver_(iksolver),
q_min_(nr_of_jnts),
q_max_(nr_of_jnts),
q_dot_min_(nr_of_jnts),
q_dot_max_(nr_of_jnts),
maxiter_(maxiter),
eps_(eps),
q_dot_(nr_of_jnts),
q_dot_old_(nr_of_jnts),
q_dot_new_(nr_of_jnts),
q_out_old_(nr_of_jnts)
{
assert(q_min.rows() == nr_of_jnts);
assert(q_max.rows() == nr_of_jnts);
assert(q_dot_max.rows() == nr_of_jnts);
assert(q_out_old_.rows() == nr_of_jnts);
SetToZero(q_out_old_);
q_min_ = q_min;
q_max_ = q_max;
q_dot_min_ = q_dot_min;
q_dot_max_ = q_dot_max;
x_dot_trans_max_ = x_dot_trans_max;
x_dot_rot_max_ = x_dot_rot_max;
x_dot_trans_min_ = x_dot_trans_min;
x_dot_rot_min_ = x_dot_rot_min;
low_pass_factor_ = low_pass_factor;
low_pass_adj_factor_ = 0.0;
nr_of_still_endeffectors_ = 0;
small_task_space_movement_ = false;
for (size_t i = 0; i < endpoints.size(); i++)
{
frames_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
old_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
frames_pos_lim_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
frames_vel_lim_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
p_in_old_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
}
}
/*---------------------------------------------------------------
arkLapackBandSetup does the setup operations for the band linear
solver. It makes a decision whether or not to call the Jacobian
evaluation routine based on various state variables, and if not
it uses the saved copy. In any case, it constructs the Newton
matrix A = M - gamma*J, updates counters, and calls the band LU
factorization routine.
---------------------------------------------------------------*/
static int arkLapackBandSetup(ARKodeMem ark_mem, int convfail,
N_Vector yP, N_Vector fctP,
booleantype *jcurPtr, N_Vector tmp1,
N_Vector tmp2, N_Vector tmp3)
{
ARKDlsMem arkdls_mem;
ARKDlsMassMem arkdls_mass_mem;
realtype dgamma, fact, *Acol_j, *Mcol_j;
booleantype jbad, jok;
int ier, retval, one = 1;
int intn, iml, imu, lenmat, ldmat, i, j, colSize;
arkdls_mem = (ARKDlsMem) ark_mem->ark_lmem;
intn = (int) arkdls_mem->d_n;
iml = (int) arkdls_mem->d_ml;
imu = (int) arkdls_mem->d_mu;
lenmat = arkdls_mem->d_M->ldata;
ldmat = arkdls_mem->d_M->ldim;
/* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
dgamma = SUNRabs((ark_mem->ark_gamma/ark_mem->ark_gammap) - ONE);
jbad = (ark_mem->ark_nst == 0) ||
(ark_mem->ark_nst > arkdls_mem->d_nstlj + ARKD_MSBJ) ||
((convfail == ARK_FAIL_BAD_J) && (dgamma < ARKD_DGMAX)) ||
(convfail == ARK_FAIL_OTHER);
jok = !jbad;
/* If jok = TRUE, use saved copy of J */
if (jok) {
*jcurPtr = FALSE;
dcopy_f77(&lenmat, arkdls_mem->d_savedJ->data,
&one, arkdls_mem->d_M->data, &one);
/* If jok = FALSE, call jac routine for new J value */
} else {
arkdls_mem->d_nje++;
arkdls_mem->d_nstlj = ark_mem->ark_nst;
*jcurPtr = TRUE;
SetToZero(arkdls_mem->d_M);
retval = arkdls_mem->d_bjac(arkdls_mem->d_n, arkdls_mem->d_mu,
arkdls_mem->d_ml, ark_mem->ark_tn,
yP, fctP, arkdls_mem->d_M,
arkdls_mem->d_J_data, tmp1, tmp2, tmp3);
if (retval == 0) {
dcopy_f77(&lenmat, arkdls_mem->d_M->data, &one,
arkdls_mem->d_savedJ->data, &one);
} else if (retval < 0) {
arkProcessError(ark_mem, ARKDLS_JACFUNC_UNRECVR, "ARKLAPACK",
"arkLapackBandSetup", MSGD_JACFUNC_FAILED);
arkdls_mem->d_last_flag = ARKDLS_JACFUNC_UNRECVR;
return(-1);
} else if (retval > 0) {
arkdls_mem->d_last_flag = ARKDLS_JACFUNC_RECVR;
return(1);
}
}
/* Scale J by -gamma */
fact = -ark_mem->ark_gamma;
dscal_f77(&lenmat, &fact, arkdls_mem->d_M->data, &one);
/* Add mass matrix to get A = M-gamma*J*/
if (ark_mem->ark_mass_matrix) {
/* Compute mass matrix */
arkdls_mass_mem = (ARKDlsMassMem) ark_mem->ark_mass_mem;
SetToZero(arkdls_mass_mem->d_M);
retval = arkdls_mass_mem->d_bmass(arkdls_mass_mem->d_n, arkdls_mass_mem->d_mu,
arkdls_mass_mem->d_ml, ark_mem->ark_tn,
arkdls_mass_mem->d_M, arkdls_mass_mem->d_M_data,
tmp1, tmp2, tmp3);
arkdls_mass_mem->d_nme++;
if (retval < 0) {
arkProcessError(ark_mem, ARKDLS_MASSFUNC_UNRECVR, "ARKLAPACK",
"arkLapackBandSetup", MSGD_MASSFUNC_FAILED);
arkdls_mem->d_last_flag = ARKDLS_MASSFUNC_UNRECVR;
return(-1);
}
if (retval > 0) {
arkdls_mem->d_last_flag = ARKDLS_MASSFUNC_RECVR;
return(1);
}
/* Add to A -- CURRENTLY ASSUMES THAT BOTH MATRICES HAVE
THE SAME BAND STRUCTURE AND COLUMN SIZE */
colSize = arkdls_mem->d_M->mu + arkdls_mem->d_M->ml + 1;
for (j=0; j<arkdls_mem->d_M->M; j++) {
Acol_j = arkdls_mem->d_M->cols[j] + arkdls_mem->d_M->s_mu - arkdls_mem->d_M->mu;
Mcol_j = arkdls_mass_mem->d_M->cols[j] + arkdls_mass_mem->d_M->s_mu
- arkdls_mass_mem->d_M->mu;
for (i=0; i<colSize; i++)
Acol_j[i] += Mcol_j[i];
}
} else {
AddIdentity(arkdls_mem->d_M);
}
/* Do LU factorization of M */
dgbtrf_f77(&intn, &intn, &iml, &imu, arkdls_mem->d_M->data,
&ldmat, arkdls_mem->d_pivots, &ier);
/* Return 0 if the LU was complete; otherwise return 1 */
arkdls_mem->d_last_flag = (long int) ier;
if (ier > 0) return(1);
return(0);
}
static int cvDenseSetup(CVodeMem cv_mem, int convfail, N_Vector ypred,
N_Vector fpred, booleantype *jcurPtr,
N_Vector vtemp1, N_Vector vtemp2, N_Vector vtemp3)
{
CVDlsMem cvdls_mem;
booleantype jbad, jok;
realtype dgamma;
int retval;
long int ier;
cvdls_mem = (CVDlsMem) lmem;
/* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
dgamma = ABS((gamma/gammap) - ONE);
jbad = (nst == 0) || (nst > nstlj + CVD_MSBJ) ||
((convfail == CV_FAIL_BAD_J) && (dgamma < CVD_DGMAX)) ||
(convfail == CV_FAIL_OTHER);
jok = !jbad;
if (jok) {
/* If jok = TRUE, use saved copy of J */
*jcurPtr = FALSE;
DenseCopy(savedJ, M);
} else {
/* If jok = FALSE, call jac routine for new J value */
nje++;
nstlj = nst;
*jcurPtr = TRUE;
SetToZero(M);
retval = jac(n, tn, ypred, fpred, M, J_data, vtemp1, vtemp2, vtemp3);
if (retval < 0) {
cvProcessError(cv_mem, CVDLS_JACFUNC_UNRECVR, "CVSDENSE", "cvDenseSetup", MSGD_JACFUNC_FAILED);
last_flag = CVDLS_JACFUNC_UNRECVR;
return(-1);
}
if (retval > 0) {
last_flag = CVDLS_JACFUNC_RECVR;
return(1);
}
DenseCopy(M, savedJ);
}
/* Scale and add I to get M = I - gamma*J */
DenseScale(-gamma, M);
AddIdentity(M);
/* Do LU factorization of M */
ier = DenseGETRF(M, lpivots);
/* Return 0 if the LU was complete; otherwise return 1 */
last_flag = ier;
if (ier > 0) return(1);
return(0);
}
int main(){
double pi = 3.14;
SetToZero(pi);
printf("pi的值:%.2f\n",pi);
}
