TestState() :
x(0.0),
v(0.0)
{
Covariance.resize(ndim(), ndim());
Covariance.setIdentity();
};
void FixedAxisOneDoFJoint::v2qdot(double* q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
if (dv_to_qdot) {
dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
}
}
void FixedAxisOneDoFJoint::qdot2v(double* q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
if (dqdot_to_v) {
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
}
}
void projectorDummy(const Eigen::MatrixXd& pseudoInv,
const Eigen::MatrixXd& jac,
Eigen::MatrixXd& result)
{
result.setIdentity();
result.noalias() -= pseudoInv*jac;
}
void RollPitchYawFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
if (dv_to_qdot) {
dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
}
}
void RollPitchYawFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
if (dqdot_to_v) {
dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
}
}
PitchRollState()
{
Omega.setZero();
GyroBias.setZero();
AccelBias.setZero();
Covariance.resize(ndim(), ndim());
Covariance.setIdentity();
};
void FingerKinematics::initialise(KDL::Chain &chain,const std::string name){
this->name = name;
for(std::size_t i = 0; i < chain.segments.size();i++){
joint_names.push_back(chain.segments[i].getJoint().getName());
}
ik_solver_vel_wdls = new KDL::ChainIkSolverVel_wdls(chain, 0.001, 5);
ik_solver_vel_wdls->setLambda(0.01);
Eigen::MatrixXd TS; TS.resize(6,6); TS.setIdentity(); TS(3,3) = 0; TS(4,4) = 0; TS(5,5) = 0;
ik_solver_vel_wdls->setWeightTS(TS);
fk_solver_pos = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_pos_NR = new KDL::ChainIkSolverPos_NR(chain,*fk_solver_pos,*ik_solver_vel_wdls);
num_joints = chain.getNrOfJoints();
q.resize(num_joints);
q.data.setZero();
q_out = q;
}
/**
* Take as input the full constraint matrix CI for a dimension ny, and
* the associated ci vector with the 2-norm of each row scaled by delta.
* Apply the quadratic program and test against all the constraints.
* Return true if all constraints match, false otherwise.
*/
int rational_fitter_parsec_multi::solve_wrapper( const gesvdm2_args_t *args, subproblem_t *pb, int M, int ny,
double *CIptr, double *ciptr )
{
const vertical_segment *d = (const vertical_segment*)(args->dataptr);
rational_function *rf = (rational_function*)(pb->rfptr);
rational_function_1d *rf1d = rf->get(ny);
const int np = pb->np;
const int nq = pb->nq;
const int N = np + nq;
// Compute the solution
Eigen::MatrixXd CE(N, 0);
Eigen::VectorXd ce(0);
Eigen::MatrixXd G (N, N); G.setIdentity();
Eigen::VectorXd g (N); g.setZero();
Eigen::VectorXd x (0.0, N);
Eigen::MatrixXd CI = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Map(CIptr, N, M);
Eigen::Map<Eigen::VectorXd> ci(ciptr, M);
// Select the size of the result vector to
// be equal to the dimension of p + q
double cost = QuadProgPP::solve_quadprog(G, g, CE, ce, CI, ci, x);
bool solves_qp = !(cost == std::numeric_limits<double>::infinity());
if(solves_qp) {
std::cout << "<<INFO>> got solution for pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
// Recopy the vector d
vec p(np), q(nq);
double norm = 0.0;
for(int i=0; (i<N) & solves_qp; ++i) {
const double v = x[i];
solves_qp = solves_qp && !isnan(v)
&& (v != std::numeric_limits<double>::infinity());
norm += v*v;
if(i < np) {
p[i] = v;
}
else {
q[i - np] = v;
}
}
if (solves_qp) {
std::cout << "<<INFO>> got solution to second step for pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
// Rq: doesn't need protection in // since it should be working on independant vectors
rf1d->update(p, q);
solves_qp = (test_all_constraint( d, rf1d, ny ) == 1);
}
}
else {
std::cerr << "<<DEBUG>> Didn't get solution to the pb with np=" << pb->np << ", nq=" << pb->nq << std::endl;
}
return solves_qp;
}
bool WholeBodyController::initialize(const double Ts)
{
ros::NodeHandle n("~");
std::string ns = n.getNamespace();
//ROS_INFO("Nodehandle %s",n.getNamespace().c_str());
ROS_INFO("Initializing whole body controller");
KDL::JntArray q_min, q_max;
if ( !ChainParser::parse(robot_state_.tree_, joint_name_to_index_, index_to_joint_name_, q_min, q_max) ) {
return false;
}
num_joints_ = joint_name_to_index_.size();
robot_state_.tree_.getJointNames(joint_name_to_index_,robot_state_.tree_.joint_name_to_index_);
robot_state_.tree_.getTreeJointIndex(robot_state_.tree_.kdl_tree_, robot_state_.tree_.tree_joint_index_);
q_current_.resize(num_joints_);
q_current_.data.setZero();
// Read parameters
std::vector<double> JLA_gain(num_joints_); // Gain for the joint limit avoidance
std::vector<double> JLA_workspace(num_joints_); // Workspace: joint gets 'pushed' back when it's outside of this part of the center of the workspace
std::vector<double> posture_q0(num_joints_);
std::vector<double> posture_gain(num_joints_);
std::vector<double> admittance_mass(num_joints_);
std::vector<double> admittance_damping(num_joints_);
for (std::map<std::string, unsigned int>::iterator iter = joint_name_to_index_.begin(); iter != joint_name_to_index_.end(); ++iter)
{
n.param<double> ("/whole_body_controller/joint_limit_avoidance/gain/"+iter->first, JLA_gain[iter->second], 1.0);
n.param<double> ("/whole_body_controller/joint_limit_avoidance/workspace/"+iter->first, JLA_workspace[iter->second], 0.9);
n.param<double> ("/whole_body_controller/posture_control/home_position/"+iter->first, posture_q0[iter->second], 0);
n.param<double> ("/whole_body_controller/posture_control/gain/"+iter->first, posture_gain[iter->second], 1.0);
n.param<double> ("/whole_body_controller/admittance_control/mass/"+iter->first, admittance_mass[iter->second], 10);
n.param<double> ("/whole_body_controller/admittance_control/damping/"+iter->first, admittance_damping[iter->second], 10);
}
loadParameterFiles();
// Construct the FK and Jacobian solvers
robot_state_.fk_solver_ = new KDL::TreeFkSolverPos_recursive(robot_state_.tree_.kdl_tree_);
robot_state_.tree_.jac_solver_ = new KDL::TreeJntToJacSolver(robot_state_.tree_.kdl_tree_);
// Initialize admittance controller
AdmitCont_.initialize(Ts,q_min, q_max, admittance_mass, admittance_damping);
// Initialize nullspace calculator
// ToDo: Make this variable
Eigen::MatrixXd A;
A.setIdentity(num_joints_,num_joints_);
ComputeNullspace_.initialize(num_joints_, A);
// Initialize Joint Limit Avoidance
//for (uint i = 0; i < num_joints_; i++) ROS_INFO("JLA gain of joint %i is %f",i,JLA_gain[i]);
JointLimitAvoidance_.initialize(q_min, q_max, JLA_gain, JLA_workspace);
ROS_INFO("Joint limit avoidance initialized");
// Initialize Posture Controller
PostureControl_.initialize(q_min, q_max, posture_q0, posture_gain, joint_name_to_index_);
ROS_INFO("Posture Control initialized");
/// Resizing variables (are set to zero in updatehook)
qdot_reference_.resize(num_joints_);
q_reference_.resize(num_joints_);
tau_.resize(num_joints_);
Jacobians_.resize(4);
Ns_.resize(4);
taus_.resize(4);
for (unsigned int i = 0; i < 4; i++) {
Jacobians_[i].resize(100, num_joints_);
Ns_[i].resize(num_joints_, num_joints_);
taus_[i].resize(num_joints_);
}
/// Initialize tracer
std::vector<std::string> column_names = index_to_joint_name_;
for (unsigned int i = 0; i < index_to_joint_name_.size(); i++) column_names.push_back("tau_cart"+index_to_joint_name_[i]);
for (unsigned int i = 0; i < index_to_joint_name_.size(); i++) column_names.push_back("tau_null"+index_to_joint_name_[i]);
for (unsigned int i = 0; i < index_to_joint_name_.size(); i++) column_names.push_back("tau_total"+index_to_joint_name_[i]);
column_names.push_back("cartesian_impedance_cost");
column_names.push_back("collision_avoidance_cost");
column_names.push_back("joint_limit_cost");
column_names.push_back("posture_cost");
int buffersize;
std::string foldername;
n.param<int> ("/whole_body_controller/tracing_buffersize", buffersize, 0);
n.param<std::string> ("/whole_body_controller/tracing_folder", foldername, "/tmp/");
std::string filename = "joints";
//std::string folderfilename = foldername + filename; // ToDo: make nice
tracer_.Initialize(foldername, filename, column_names, buffersize);
statsPublisher_.initialize();
ROS_INFO("Whole Body Controller Initialized");
return true;
}
Eigen::Isometry3d pose_estimate(FrameMatchPtr match,
std::vector<char> & inliers,
Eigen::Isometry3d & motion,
Eigen::MatrixXd & motion_covariance,
Eigen::Matrix<double, 3, 4> & proj_matrix) {
using namespace pose_estimator;
PoseEstimator pe(proj_matrix);
if ((match->featuresA_indices.size()!=match->featuresB_indices.size()))
cout << "Number of features doesn't match\n";
size_t num_matches = match->featuresA_indices.size();
if(num_matches < 3)
cout << "Need at least three matches to estimate pose";
motion.setIdentity();
motion_covariance.setIdentity();
Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::ColMajor> src_xyzw(4, num_matches);
Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::ColMajor>dst_xyzw(4, num_matches);
for (size_t i=0; i < num_matches; ++i) {
// const ImageFeature& featureA(int i) const {
// assert (frameA);
// int ix = featuresA_indices.at(i);
// return frameA->features().at(ix);
// }
//src_xyzw.col(i) = match->featureA(i).xyzw;
//dst_xyzw.col(i) = match->featureB(i).xyzw;
int ixA = match->featuresA_indices.at(i);
int ixB = match->featuresB_indices.at(i);
//cout << ixA << " | " << ixB << "\n";
//cout << match->featuresA.size() << " fA size\n";
//cout << match->featuresA[ixA].xyzw[0] << "\n";
//cout << match->featuresA[ixA].xyzw[0] << "\n";
src_xyzw.col(i) = match->featuresA[ixA].xyzw;
dst_xyzw.col(i) = match->featuresB[ixB].xyzw;
}
// PoseEstimateStatus status = pe.estimate(src_xyzw, dst_xyzw, &inliers, &motion, &motion_covariance);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> motion_covariance_col_major;
pose_estimator::PoseEstimateStatus status = pe.estimate(src_xyzw, dst_xyzw, &inliers, &motion, &motion_covariance_col_major); //&motion_covariance);
motion_covariance = motion_covariance_col_major;
match->status = status;
match->delta = motion;
/*
int num_inliers = std::accumulate(inliers.begin(), inliers.end(), 0);
const char* str_status = PoseEstimateStatusStrings[status];
std::cerr << "Motion: " << str_status << " feats: " << match->featuresA_indices.size()
<< " inliers: " << num_inliers
<< " Pose: " << motion
<< " Delta: " << match->delta
//<< " Cov:\n" << motion_covariance << "\n"
<< " " << match->timeA << " " << match->timeB << std::endl;
*/
return motion;
}
void Kinematics::test_ik(){
using namespace KDL;
KDL::Chain chain1;
chain1.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
chain1.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
chain1.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
chain1.addSegment(Segment(Joint(Joint::RotZ)));
chain1.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
chain1.addSegment(Segment(Joint(Joint::RotZ)));
print_chain(chain1);
//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain1);//Forward position solver
ChainIkSolverVel_wdls ik_solver_wdls(chain1);
Eigen::MatrixXd Mx;
Mx.resize(6,6);
Mx.setIdentity();
Mx(3,3) = 0;Mx(4,4) = 0;Mx(5,5) = 0;
std::cout<< Mx << std::endl;
// Mx.Identity();
// ik_solver_wdls.setWeightTS(Mx);
ChainIkSolverPos_NR iksolver1(chain1,fksolver1,ik_solver_wdls,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
//Creation of jntarrays:
JntArray q(chain1.getNrOfJoints());
JntArray q_init(chain1.getNrOfJoints());
q_init(0) = 0.1;
q_init(1) = 0.2;
q_init(2) = 0.3;
q_init(3) = 0.2;
q_init(4) = 0.1;
q_init(5) = 0.0;
// Create the frame that will contain the results
KDL::Frame cartpos;
// Calculate forward position kinematics
bool kinematics_status;
kinematics_status = fksolver1.JntToCart(q_init,cartpos);
if(kinematics_status>=0){
std::cout << cartpos <<std::endl;
printf("%s \n","Succes, thanks KDL!");
}else{
printf("%s \n","Error: could not calculate forward kinematics :(");
}
Frame current,target;
fksolver1.JntToCart(q_init,current);
std::cout<< "current: " << current.p(0) << " " << current.p(1) << " " << current.p(2) << std::endl;
target = current;
target.p(0) = target.p(0) + 0.1;
std::cout<< "target: " << target.p(0) << " " << target.p(1) << " " << target.p(2) << std::endl;
int ret = iksolver1.CartToJnt(q_init,target,q);
std::cout<< "ret: " << ret << std::endl;
std::cout<< "q_init: " << q_init.data << std::endl;
std::cout<< "q: " << q.data << std::endl;
}
/**
* Simulate
*
* @param double input
* @return double u - control signal
*/
double RegulatorGPC::simulate(double input)
{
if(m_proces)
{
m_w = m_proces->simulate();
const double y = input;
m_e = m_w - y;
m_history_Y.push_front(y);
m_identify.add_sample(input, m_history_U.front());
//std::deque<double> A,B;
m_poly_A.clear();
m_poly_B.clear();
//A.push_back(-0.6);
//B.push_back(0.4);
m_identify.get_polynomial_a(m_poly_A);
m_identify.get_polynomial_b(m_poly_B);
// Return disruption
if (m_initial_steps_left > 0)
{
const double u = m_w - y;
m_history_U.push_front(u);
m_initial_steps_left--;
return u;
}
// Control algorithm
// ------------------------------------------------------------------------------------------------------
// 1. Calculating h initial conditions equal zero, delay = 0
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 27
Eigen::VectorXd h(m_H);
{
std::map<std::string, double> others;
others["k"] = 0;
others["stationary"] = 0;
others["noise"] = 0;
ARX ob;
ob.set_parameters(m_poly_A,m_poly_B,others);
for(int i=0; i<m_H; i++)
{
h[i] = ob.simulate(1.0);
}
}
// 2. Calculating Q:
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 28
Eigen::MatrixXd Q;
Q.setZero(m_H, m_L);
for(int i=0; i<m_H; i++)
{
for(int j=0; j<m_L; j++)
{
if(i-j<0)
{
Q(i,j) = 0;
}
else
{
Q(i,j) = h[i-j];
}
}
}
// 3. Calculating w0
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 8
Eigen::VectorXd w0(m_H);
w0[0] = (1-m_alpha)*m_w + m_alpha*y;
for(int i=1; i<m_H; i++)
{
w0[i] = (1-m_alpha)*m_w + m_alpha*w0[i-1];
}
// 4. Calculating q
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 20
Eigen::VectorXd tmp;
tmp.setZero(m_L);
tmp[0] = 1;
Eigen::MatrixXd mIdentity;
mIdentity.setIdentity(m_L,m_L);
Eigen::VectorXd q = (tmp.transpose()
*((Q.transpose()*Q + m_ro*mIdentity).inverse())
*Q.transpose()
).transpose();
// 5. Calculating y0
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 31
Eigen::VectorXd y0(m_H);
{
ARX ob;
std::map<std::string, double> others;
others["k"] = 0;
others["stationary"] = 0;
others["noise"] = 0;
ob.set_parameters(m_poly_A,m_poly_B,others);
ob.set_initial_state(m_history_U, m_history_Y);
ob.simulate(m_history_U.front());
for (int i=0; i<m_H; i++)
{
y0[i] = ob.simulate(m_history_U.front());
}
}
// 6. Calculating final control
// http://platforma.polsl.pl/rau1/file.php/62/Cz_4_regulacja_predykcyjna.pdf
// page 35
const double du = q.transpose() * (w0-y0);
const double u = m_history_U.front() + du;
m_history_U.push_front(u);
return u;
}
else
return 0.0;
}
