/**
* transpose the matrix
*/
Matrix<N, M> transposed(void) const {
#ifdef CONFIG_ARCH_ARM
Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
(this->arm_mat.pData);
Me.transposeInPlace();
Matrix<N, M> res(Me.data());
return res;
#endif
}
void setGains(const hrl_pr2_force_ctrl::HybridCartesianGains::ConstPtr &msg) // khawkins
{
// Store gains...
if (msg->p_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kp[i] = msg->p_gains[i];
if (msg->d_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kd[i] = msg->d_gains[i];
// Store force gains... khawkins
if (msg->fp_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kfp[i] = msg->fp_gains[i];
if (msg->fi_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kfi[i] = msg->fi_gains[i];
if (msg->fi_max_gains.size() == 6)
for (size_t i = 0; i < 6; ++i)
Kfi_max[i] = msg->fi_max_gains[i];
// Store selector matricies khawkins
if (msg->force_selector.size() == 6)
for (size_t i = 0; i < msg->force_selector.size(); ++i)
if(msg->force_selector[i]) {
force_selector[i] = 1;
motion_selector[i] = 0;
} else {
force_selector[i] = 0;
motion_selector[i] = 1;
}
// Store frame information
if(!msg->header.frame_id.compare(root_name_))
{
use_tip_frame_ = false;
ROS_INFO("New gains in root frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
root_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
St.setIdentity();
}
else if(!msg->header.frame_id.compare(tip_name_))
{
use_tip_frame_ = true;
ROS_INFO("New gains in tip frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
tip_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
}
else
{
use_tip_frame_ = false;
geometry_msgs::PoseStamped in_root;
in_root.pose.orientation.w = 1.0;
in_root.header.frame_id = msg->header.frame_id;
try {
tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
tf_.transformPose(root_name_, in_root, in_root);
}
catch (const tf::TransformException &ex)
{
ROS_ERROR("Failed to transform: %s", ex.what());
return;
}
Eigen::Affine3d t;
tf::poseMsgToEigen(in_root.pose, t);
St <<
t(0,0),t(0,1),t(0,2),0,0,0,
t(1,0),t(1,1),t(1,2),0,0,0,
t(2,0),t(2,1),t(2,2),0,0,0,
0,0,0,t(0,0),t(0,1),t(0,2),
0,0,0,t(1,0),t(1,1),t(1,2),
0,0,0,t(2,0),t(2,1),t(2,2);
St.transposeInPlace();
ROS_INFO("New gains in arbitrary frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
msg->header.frame_id.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
}
}