void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
if(msg->header.frame_id.compare(frame_id_) != 0)
{
ROS_WARN("frame_id of goal did not match expected '%s'. Ignoring goal",
frame_id_.c_str());
return;
}
// copying over goal
state_[joint_names_.size() + 0] = msg->pose.position.x;
state_[joint_names_.size() + 1] = msg->pose.position.y;
state_[joint_names_.size() + 2] = msg->pose.position.z;
KDL::Rotation rot;
tf::quaternionMsgToKDL(msg->pose.orientation, rot);
rot.GetEulerZYX(state_[joint_names_.size() + 3], state_[joint_names_.size() + 4],
state_[joint_names_.size() + 5]);
if (!controller_started_)
{
if (controller_.start(state_, nWSR_))
{
ROS_INFO("Controller started.");
controller_started_ = true;
}
else
{
ROS_ERROR("Couldn't start controller.");
print_eigen(state_);
}
}
}
void bend (double **mat, double **mat_ext, int tr)
{
int j;
double **v, *d;
v = (double **) malloc (tr*sizeof(double *));
for (j = 0; j < tr; j++)
v[j] = (double *) malloc (tr*sizeof(double));
d = (double *) malloc (tr*sizeof(double));
jacobi(mat, v, d, tr);
print_eigen (v, d, tr);
}
