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); }