vec SMCLaw::Doit(double t, vec q0_, vec q1_){
Dy *dy;
dy = R2->Doit(q0_, q1_);
mat Aux_;
if(RefObjFlag){
RefObj->Doit(t);
s_ = - ( (RefObj->dr_ - R2->P->q1_ ) + Kp*(RefObj->r_ - R2->P->q0_ ) );
sigma_ = RefObj->d2r_ + Kp*(RefObj->dr_ - R2->P->q1_ );
}
else{
s_ = - ( (dr_(t) - R2->P->q1_ ) + Kp*(r_(t) - R2->P->q0_ ) );
sigma_ = d2r_(t) + Kp*(dr_(t) - R2->P->q1_ );
}
switch(caso){
case 1: k_ = eta*ones(dof);
break;
case 2: k = eta + abs(R2->P->q1_).t()*Lambda_*abs(R2->P->q1_) + gamma_.t()*abs(sigma_);
k_ = k(0,0)*ones(dof);
break;
case 3: k_ = eta_ + Gamma_*abs(sigma_);
for(uint i=0; i<dof; i++){
k = abs(R2->P->q1_).t()*Lambda__.slice(i)*abs(R2->P->q1_);
k_(i) += k(0,0);
}
break;
}
return dy->vh_ + dy->gh_ + dy->Mh_*(sigma_ - k_ % tanh(n*s_) );
}
void AdmittanceController::update(const Eigen::VectorXd& tau, Eigen::VectorXd& qdot_reference, const KDL::JntArray& q_current, Eigen::VectorXd& q_reference) {
// Update filters --> as a result desired velocities are known
for (int i = 0; i<tau.size(); i++) {
/// Check input values (and create segfault to debug)
if (!(fabs(tau(i)) < 10000.0 && fabs(qdot_reference_previous_(i)) < 10000.0) ) {
std::cout << "tauin: " << tau(i) << ", qdot: " << qdot_reference_previous_(i) << std::endl;
assert(false);
} else {
qdot_reference(i) = b_(0,i) * tau(i);
qdot_reference(i) += b_(1,i) * tau_previous_(i);
qdot_reference(i) -= a_(1,i) * qdot_reference_previous_(i);
qdot_reference(i) *= k_(i);
// Integrate desired velocities and limit outputs
q_reference(i) = std::min(q_max_(i),std::max(q_min_(i),q_current(i)+Ts_*qdot_reference(i)));
}
}
tau_previous_ = tau;
qdot_reference_previous_ = qdot_reference;
//ROS_INFO("qdr = %f %f %f %f", qdot_reference(0), qdot_reference(1), qdot_reference(2), qdot_reference(3));
///ROS_INFO("qdrspindle = %f", qdot_reference(7));
}
Foam::scalar Foam::IrreversibleReaction<ReactionThermo, ReactionRate>::kf
(
const scalar T,
const scalar p,
const scalarField& c
) const
{
return k_(T, p, c);
}
void DynamicSlidingModeControllerTaskSpace::starting(const ros::Time& time)
{
// 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();
joint_des_states_.q(i) = joint_msr_states_.q(i);
sigma_(i) = 0;
// coefficients > 0
Kd_(i) = 10;
gamma_(i) = 1;
alpha_(i) = 12;
lambda_(i) = 1;
k_(i) = 3;
}
//x_des_ = KDL::Frame(KDL::Rotation::RPY(0,0,0),KDL::Vector(-0.4,0.3,1.5));
cmd_flag_ = 0;
step_ = 0;
}
int main()
{
klass k;
klass k_(k);
}
Real k() const {
return k_(0.0);
}
void AdmittanceController::initialize(const double Ts, const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& mass, const std::vector<double>& damping) {
// Resize relevant parameters
uint num_joints = 15;
Ts_ = Ts;
// ToDo: Make variables (use M and D as inputs?)
m_.resize(num_joints);
d_.resize(num_joints);
k_.resize(num_joints);
om_.resize(num_joints);
a_.resize(2,num_joints);
b_.resize(2,num_joints);
tau_previous_.resize(num_joints);
qdot_reference_previous_.resize(num_joints);
q_min_.resize(num_joints);
q_max_.resize(num_joints);
// Set parameters
/*(hardcoded)
for (uint i = 0; i<num_joints; i++) {
m_(i) = 0.1;
d_(i) = 1;
}
m_(7) = 1; // Torso
d_(7) = 10; // Torso
*/
for (uint i = 0; i < num_joints; i++) {
m_(i) = mass[i];
d_(i) = damping[i];
}
for (uint i = 0; i<num_joints; i++) {
k_(i) = 1/d_(i);
om_(i) = d_(i)/m_(i);
double wp = om_(i) + eps;
double alpha = wp/(tan(wp*Ts_/2));
double x1 = alpha/om_(i)+1;
double x2 = -alpha/om_(i)+1;
// Numerator and denominator of the filter
a_(0,i) = 1;
a_(1,i) = x2 / x1;
b_(0,i) = 1 / x1;
b_(1,i) = 1 / x1;
///ROS_INFO("a %i = %f, %f, b %i = %f, %f", i, a_(0,i), a_(1,i), i, b_(0,1), b_(1,i));
// Set previous in- and outputs to zero
tau_previous_(i) = 0;
qdot_reference_previous_(i) = 0;
// Set joint limits
q_min_(i) = q_min(i);
q_max_(i) = q_max(i);
}
// Set inputs, states, outputs to zero
ROS_INFO("Admittance controller initialized");
}
void DynamicSlidingModeControllerTaskSpace::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();
}
if (cmd_flag_)
{
// computing forward kinematics
fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);
// computing Jacobian J(q)
jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);
// computing Jacobian pseudo-inversion
pseudo_inverse(J_.data,J_pinv_);
// computing end-effector position/orientation error w.r.t. desired frame
x_err_ = diff(x_,x_des_);
/* Trying quaternions, it seems to work better
// end-effector position/orientation error
x_err_.vel = (x_des_.p - x_.p);
// getting quaternion from rotation matrix
x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);
skew_symmetric(quat_des_.v,skew_);
for (int i = 0; i < skew_.rows(); i++)
{
v_temp_(i) = 0.0;
for (int k = 0; k < skew_.cols(); k++)
v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
}
x_err_.rot = (quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v) - v_temp_;
*/
// clearing error msg before publishing
msg_err_.data.clear();
for(int i = 0; i < e_ref_.size(); i++)
{
e_ref_(i) = x_err_(i);
msg_err_.data.push_back(e_ref_(i));
}
joint_des_states_.qdot.data = J_pinv_*e_ref_;
joint_des_states_.q.data = joint_msr_states_.q.data + period.toSec()*joint_des_states_.qdot.data;
// computing S
S_.data = (joint_msr_states_.qdot.data - joint_des_states_.qdot.data) + alpha_.data.cwiseProduct(joint_msr_states_.q.data - joint_des_states_.q.data);
//for (int i = 0; i < joint_handles_.size(); i++)
//S_(i) = (joint_msr_states_.qdot(i) - joint_des_states_.qdot(i)) + alpha_(i)*tanh(lambda_(i)*(joint_msr_states_.q(i) - joint_des_states_.q(i)));
// saving S0 on the first step
if (step_ == 0)
S0_ = S_;
// computing Sd
for (int i = 0; i < joint_handles_.size(); i++)
Sd_(i) = S0_(i)*exp(-k_(i)*(step_*period.toSec()));
Sq_.data = S_.data + Sd_.data;//- Sd_.data;
// computing sigma_dot as sgn(Sq)
for (int i = 0; i < joint_handles_.size(); i++)
sigma_dot_(i) = -(Sq_(i) < 0) + (Sq_(i) > 0);
// integrating sigma_dot
sigma_.data += period.toSec()*sigma_dot_.data;
//for (int i = 0; i < joint_handles_.size(); i++)
//sigma_(i) += period.toSec()*pow(Sq_(i),0.5);
// computing Sr
Sr_.data = Sq_.data + gamma_.data.cwiseProduct(sigma_.data);
// computing tau
tau_.data = -Kd_.data.cwiseProduct(Sr_.data);
step_++;
if (Equal(x_,x_des_,0.05))
{
ROS_INFO("On target");
cmd_flag_ = 0;
return;
}
}
// set controls for joints
for (int i = 0; i < joint_handles_.size(); i++)
{
if (!cmd_flag_)
tau_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);//Kd_(i)*(alpha_(i)*(joint_des_states_.q(i) - joint_msr_states_.q(i)) + (joint_des_states_.qdot(i) - joint_msr_states_.qdot(i))) ;
joint_handles_[i].setCommand(tau_(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_);
// publishing actual and desired trajectory for each task (links) as an array of ntasks*3
pub_pose_.publish(msg_pose_);
pub_traj_.publish(msg_traj_);
ros::spinOnce();
}
void PhaseResidual<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{
typedef Intrepid::FunctionSpaceTools FST;
FST::scalarMultiplyDataData<ScalarT> (term1_,k_,T_grad_);
FST::integrate<ScalarT>(residual_,term1_,w_grad_bf_,Intrepid::COMP_CPP,false);
FST::scalarMultiplyDataData<ScalarT> (term2_,rho_cp_,T_dot_);
FST::integrate<ScalarT>(residual_,term2_,w_bf_,Intrepid::COMP_CPP,true);
for (int i=0; i<source_.size(); ++i)
source_[i] *= -1.0;
FST::integrate<ScalarT>(residual_,source_,w_bf_,Intrepid::COMP_CPP,true);
for (int i=0; i<laser_source_.size(); ++i)
laser_source_[i] *= -1.0;
FST::integrate<ScalarT>(residual_,laser_source_,w_bf_,Intrepid::COMP_CPP,true);
/*
std::cout<<"rho Cp values"<<std::endl;
for (std::size_t cell = 0; cell < workset.numCells; ++cell) {
for (std::size_t qp = 0; qp < num_qps_; ++qp) {
std::cout<<rho_cp_(cell,qp)<<" ";
}
std::cout<<std::endl;
}
std::cout<<"Thermal cond values"<<std::endl;
for (std::size_t cell = 0; cell < workset.numCells; ++cell) {
for (std::size_t qp = 0; qp < num_qps_; ++qp) {
std::cout<<k_(cell,qp)<<" ";
}
std::cout<<std::endl;
}
*/
//----------------------------
#if 0
for (std::size_t cell = 0; cell < workset.numCells; ++cell) {
for (std::size_t qp = 0; qp < num_qps_; ++qp) {
term2_(cell,qp) = rho_cp_(cell,qp)*T_dot_(cell,qp);
}
}
#endif
// no rho_cp_ term - equivalent to heat problem
// FST::integrate<ScalarT>(residual_,T_dot_,w_bf_,Intrepid::COMP_CPP,true);
#if 0
// temperature residual
for (std::size_t cell = 0; cell < workset.numCells; ++cell) {
for (std::size_t node = 0; node < num_nodes_; ++node) {
residual_(cell,node) = 0.0;
}
for (std::size_t qp = 0; qp < num_qps_; ++qp) {
for (std::size_t node = 0; node < num_nodes_; ++node) {
for (std::size_t i = 0; i < num_dims_; ++i) {
residual_(cell,node) +=
k_(cell,qp) * T_grad_(cell,qp,i) * w_grad_bf_(cell,node,qp,i) +
rho_cp_(cell,qp) * T_dot_(cell,qp) * w_bf_(cell,node,qp);
}
}
}
}
// source function
for (std::size_t cell = 0; cell < workset.numCells; ++cell) {
for (std::size_t qp = 0; qp < num_qps_; ++qp) {
for (std::size_t node = 0; node < num_nodes_; ++node) {
residual_(cell,node) -=
source_(cell,qp) * w_bf_(cell,node,qp);
}
}
}
#endif
}
void JointLimitAvoidance::updateHook() {
if (port_joint_position_.read(joint_position_) != RTT::NewData) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "could not read port: " << port_joint_position_.getName() << RTT::endlog();
return;
}
if (joint_position_.size() != number_of_joints_) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "wrong data size: " << joint_position_.size()
<< " on port: " << port_joint_position_.getName()
<< RTT::endlog();
return;
}
if (port_joint_velocity_.read(joint_velocity_) != RTT::NewData) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "could not read port: " << port_joint_velocity_.getName() << RTT::endlog();
return;
}
if (joint_velocity_.size() != number_of_joints_) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "wrong data size: " << joint_velocity_.size()
<< " on port: " << port_joint_velocity_.getName()
<< RTT::endlog();
return;
}
if (port_nullspace_torque_command_.read(nullspace_torque_command_) != RTT::NewData) {
// this is acceptable
nullspace_torque_command_.setZero();
}
if (nullspace_torque_command_.size() != number_of_joints_) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "wrong data size: " << nullspace_torque_command_.size()
<< " on port: " << port_nullspace_torque_command_.getName()
<< RTT::endlog();
return;
}
for (size_t i = 0; i < number_of_joints_; i++) {
joint_torque_command_(i) = jointLimitTrq(upper_limit_[i],
lower_limit_[i], limit_range_[i], max_trq_[i],
joint_position_(i));
if (abs(joint_torque_command_(i)) > 0.001) {
k_(i) = max_trq_[i]/limit_range_[i];
} else {
k_(i) = 0.001;
}
}
if (port_mass_matrix_.read(m_) != RTT::NewData) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "could not read port: " << port_mass_matrix_.getName() << RTT::endlog();
return;
}
if (m_.cols() != number_of_joints_ || m_.rows() != number_of_joints_) {
RTT::Logger::In in("JointLimitAvoidance::updateHook");
error();
RTT::log(RTT::Error) << "invalid mass matrix size: " << m_.cols() << " " << m_.rows() << RTT::endlog();
return;
}
tmpNN_ = k_.asDiagonal();
es_.compute(tmpNN_, m_);
q_ = es_.eigenvectors().inverse();
k0_ = es_.eigenvalues();
tmpNN_ = k0_.cwiseSqrt().asDiagonal();
d_.noalias() = 2.0 * q_.adjoint() * 0.7 * tmpNN_ * q_;
joint_torque_command_.noalias() -= d_ * joint_velocity_;
joint_torque_command_.noalias() += nullspace_torque_command_;
port_joint_torque_command_.write(joint_torque_command_);
}