void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) {
const double q0 = q.w();
const double q1 = q.x();
const double q2 = q.y();
const double q3 = q.z();
roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
pitch = asin(2*(q0*q2-q3*q1));
yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}
void UpperBodyPlanner::msgPose2Eigen(const geometry_msgs::Pose& input, Eigen::Vector3d& translation, Eigen::Quaterniond& q) {
translation(0) = input.position.x;
translation(1) = input.position.y;
translation(2) = input.position.z;
q.w() = input.orientation.w;
q.x() = input.orientation.x;
q.y() = input.orientation.y;
q.z() = input.orientation.z;
}
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
{
// // is it faster to do this?
k.GetQuaternion(e.x(), e.y(), e.z(), e.w());
// or this?
//double x, y, z, w;
//k.GetQuaternion(x, y, z, w);
//e = Eigen::Quaterniond(w, x, y, z);
}
Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion)
{
Eigen::Vector3d vec = quaternion.vec();
if (vec.norm() < ITOMP_EPS)
return Eigen::Vector3d::Zero();
double theta = 2.0 * std::acos(quaternion.w());
vec.normalize();
return theta * vec;
}
Eigen::Isometry3d KDLToEigen(KDL::Frame tf)
{
Eigen::Isometry3d tf_out;
tf_out.setIdentity();
tf_out.translation() << tf.p[0], tf.p[1], tf.p[2];
Eigen::Quaterniond q;
tf.M.GetQuaternion(q.x(), q.y(), q.z(), q.w());
tf_out.rotate(q);
return tf_out;
}
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
}
/*!
* \brief affine3d2UrdfPose converts an Eigen affine 4x4 matrix o represent the pose into a urdf pose
* vparam pose eigen Affine3d pose
* \return urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const Eigen::Affine3d &pose) {
RCS::Pose p;
p.getOrigin().setX(pose.translation().x());
p.getOrigin().setY(pose.translation().y());
p.getOrigin().setZ(pose.translation().z());
Eigen::Quaterniond q (pose.rotation());
tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
//std::cout << "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;
p.setRotation(qtf);
//std::cout << "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;
#if 0
MatrixEXd m = pose.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(m);
Eigen::Quaterniond q(pose.rotation());
p.getRotation().setX(q.x());
p.getRotation().setY(q.y());
p.getRotation().setZ(q.z());
p.getRotation().setW(q.w());
#endif
return p;
}
void updateVizMarker(int id, Eigen::Vector3d _pose, Eigen::Quaterniond _attitude)
{
marker[id].pose.position.x = _pose(0);
marker[id].pose.position.y = _pose(1);
marker[id].pose.position.z = _pose(2);
marker[id].pose.orientation.x = _attitude.x();
marker[id].pose.orientation.y = _attitude.y();
marker[id].pose.orientation.z = _attitude.z();
marker[id].pose.orientation.w = _attitude.w();
pub_body[id].publish( marker[id] );
}
/**
* Recursion method to be used with traverseTreeTopDown() and recursion parameters
* of type *Vector3RecursionParams*.
*
* Re-arranges the joint-transform of the recursion link's *parent joint*, along with
* the link's visual/collision/intertial rotations, such that all joints rotate around the axis
* given in the recursion parameters vector.
*/
int allRotationsToAxisCB(urdf_traverser::RecursionParamsPtr& p)
{
urdf_traverser::LinkPtr link = p->getLink();
if (!link)
{
ROS_ERROR("allRotationsToAxis: NULL link passed");
return -1;
}
Vector3RecursionParams::Ptr param = baselib_binding_ns::dynamic_pointer_cast<Vector3RecursionParams>(p);
if (!param)
{
ROS_ERROR("Wrong recursion parameter type");
return -1;
}
urdf_traverser::JointPtr joint = link->parent_joint;
if (!joint)
{
ROS_INFO_STREAM("allRotationsToAxis: Joint for link " << link->name << " is NULL, so this must be the root joint");
return 1;
}
Eigen::Vector3d axis = param->vec;
Eigen::Quaterniond alignAxis;
if (urdf_traverser::jointTransformForAxis(joint, axis, alignAxis))
{
Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
// ROS_INFO_STREAM("Transforming axis "<<rotAxis<<" for joint "<<joint->name<<" with transform "<<urdf_traverser::EigenTransform(alignAxis));
urdf_traverser::applyTransform(joint, urdf_traverser::EigenTransform(alignAxis), false);
// the link has to receive the inverse transform, so it stays at the original position
Eigen::Quaterniond alignAxisInv = alignAxis.inverse();
urdf_traverser::applyTransform(link, urdf_traverser::EigenTransform(alignAxisInv), true);
// we also have to fix the child joint's (1st order child joints) transform
// to correct for this transformation.
for (std::vector<urdf_traverser::JointPtr>::iterator pj = link->child_joints.begin();
pj != link->child_joints.end(); pj++)
{
urdf_traverser::applyTransform(*pj, urdf_traverser::EigenTransform(alignAxisInv), true);
}
// finally, set the rotation axis to the target
joint->axis.x = axis.x();
joint->axis.y = axis.y();
joint->axis.z = axis.z();
}
// all good, indicate that recursion can continue
return 1;
}
geometry_msgs::Pose UpperBodyPlanner::Eigen2msgPose(const Eigen::Vector3d& translation, const Eigen::Quaterniond& q) {
geometry_msgs::Pose out_pose;
out_pose.position.x = translation(0);
out_pose.position.y = translation(1);
out_pose.position.z = translation(2);
out_pose.orientation.w = q.w();
out_pose.orientation.x = q.x();
out_pose.orientation.y = q.y();
out_pose.orientation.z = q.z();
return out_pose;
}
double quaternion_get_yaw(const Eigen::Quaterniond &q)
{
// to match equation from:
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
const double &q0 = q.w();
const double &q1 = q.x();
const double &q2 = q.y();
const double &q3 = q.z();
return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
}
double igl::angular_distance(
const Eigen::Quaterniond & A,
const Eigen::Quaterniond & B)
{
using namespace igl;
assert(fabs(A.norm()-1)<FLOAT_EPS && "A should be unit norm");
assert(fabs(B.norm()-1)<FLOAT_EPS && "B should be unit norm");
//// acos is always in [0,2*pi)
//return acos(fabs(A.dot(B)));
return fmod(2.*acos(A.dot(B)),2.*PI);
}
Eigen::Quaterniond UpperBodyPlanner::Rmat2Quaternion(const Eigen::Matrix3d& inMat_) {
Eigen::Quaterniond outQ;
KDL::Rotation convert;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
convert.data[3 * i + j] = inMat_(i,j);
}
}
convert.GetQuaternion(outQ.x(), outQ.y(), outQ.z(), outQ.w());
return outQ;
}
void planning_models::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
{
Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
Eigen::Quaterniond q;
quatFromMsg(transform.transform.rotation, q);
setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
} else {
ROS_ERROR("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
}
}
void create_data(Eigen::MatrixXd &pa, Eigen::MatrixXd &pb) {
int n_data = 7;
Eigen::MatrixXd pa0(3, n_data);
Eigen::MatrixXd pb0(3, n_data);
pb0 << -0.7045189014502934,0.31652495664145264,-0.8913587885243552,0.4196143278053829,0.33125081405575785,-1.148712511573519,-0.7211957446166447,-0.4204243223315903,-0.8922857301575797,0.41556308950696674,-0.36760757371251074,-1.1630155401570457,-0.12535642300333297,0.26708755761917147,1.5095344824450356,0.9968448409012386,0.27593113974268946,1.2189108175890786,-0.28095118914331707,-0.40276257201497045,1.3669272703783852;
Eigen::Quaterniond q = Eigen::Quaterniond(2.86073, 0.0378363, 3.59752, 0.4211619).normalized();
std::cout << "groundtruth-quaternion. w: " << q.w() << " x " << q.x() << " y: " << q.y() << " z " << q.z() << std::endl;
pa = q.toRotationMatrix()*pb0;
pb = pb0;
}
void robot_state::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
{
Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
Eigen::Quaterniond q;
tf::quaternionMsgToEigen(transform.transform.rotation, q);
setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
} else {
logError("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
}
}
void LCM2ROS::neckPitchHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const drc::neck_pitch_t* msg)
{
ROS_ERROR("LCM2ROS got desired neck pitch");
ihmc_msgs::HeadOrientationPacketMessage mout;
Eigen::Quaterniond quat = euler_to_quat(0, msg->pitch, 0);
mout.trajectory_time = 1;
mout.orientation.w = quat.w();
mout.orientation.x = quat.x();
mout.orientation.y = quat.y();
mout.orientation.z = quat.z();
mout.unique_id = msg->utime;
neck_orientation_pub_.publish(mout);
}
void imuCallback(const sensor_msgs::Imu::ConstPtr& imu){
q_imu.w() = imu->orientation.w;
q_imu.x() = imu->orientation.x;
q_imu.y() = imu->orientation.y;
q_imu.z() = imu->orientation.z;
d_roll = imu->angular_velocity.x;
d_pitch = imu->angular_velocity.y;
d_yaw = imu->angular_velocity.z;
ax = imu->linear_acceleration.x;
ay = imu->linear_acceleration.y;
az = imu->linear_acceleration.z;
}
Eigen::Matrix3d FeatureModel::dR_by_dqz(const Eigen::Quaterniond &q)
{
Eigen::Matrix3d M;
M << -2*q.z(), -2*q.w(), 2*q.x(),
2*q.w(), -2*q.z(), 2*q.y(),
2*q.x(), 2*q.y(), 2*q.z();
return M;
}
inline void ICP::UpdateXopt(Eigen::VectorXd &xopt, const Eigen::Matrix4d& Tr){
Eigen::Quaterniond q (Tr.block<3,3>(0,0));
xopt(0) = q.x(); //rx
xopt(1) = q.y(); //ry
xopt(2) = q.z(); //rz
xopt(3) = q.w(); //teta
xopt(4) = Tr(0,3); //tx
xopt(5) = Tr(1,3); //ty
xopt(6) = Tr(2,3); //tz
}
void VideoIMUFusionDevice::handleIMUVelocity(
const OSVR_TimeValue ×tamp, const OSVR_AngularVelocityReport &report) {
using namespace osvr::util::eigen_interop;
Eigen::Quaterniond q = map(report.state.incrementalRotation);
Eigen::Vector3d rot;
if (q.w() >= 1. || q.vec().isZero(1e-10)) {
rot = Eigen::Vector3d::Zero();
} else {
#if 0
auto magnitude = q.vec().blueNorm();
rot = (q.vec() / magnitude * (2. * std::atan2(magnitude, q.w()))) /
report.state.dt;
#else
auto angle = std::acos(q.w());
rot = q.vec().normalized() * angle * 2 / report.state.dt;
#endif
}
m_fusion.handleIMUVelocity(timestamp, rot);
if (m_fusion.running()) {
sendMainPoseReport();
}
sendVelocityReport();
}
int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
double gripper_opening,
double gripper_pitch,
double x_offset,
double z_offset,
double quality)
{
moveit_msgs::Grasp grasp;
grasp.grasp_pose = pose;
// defaults
grasp.pre_grasp_posture = makeGraspPosture(gripper_opening);
grasp.grasp_posture = makeGraspPosture(0.0);
grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_,
approach_min_translation_,
approach_desired_translation_);
grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_,
retreat_min_translation_,
retreat_desired_translation_,
-1.0); // retreat is in negative x direction
// initial pose
Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
pose.pose.position.y,
pose.pose.position.z) *
Eigen::Quaterniond(pose.pose.orientation.w,
pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z);
// translate by x_offset, 0, z_offset
p = p * Eigen::Translation3d(x_offset, 0, z_offset);
// rotate by 0, pitch, 0
p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
// apply grasp point -> planning frame offset
p = p * Eigen::Translation3d(-tool_offset_, 0, 0);
grasp.grasp_pose.pose.position.x = p.translation().x();
grasp.grasp_pose.pose.position.y = p.translation().y();
grasp.grasp_pose.pose.position.z = p.translation().z();
Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
grasp.grasp_pose.pose.orientation.x = q.x();
grasp.grasp_pose.pose.orientation.y = q.y();
grasp.grasp_pose.pose.orientation.z = q.z();
grasp.grasp_pose.pose.orientation.w = q.w();
grasp.grasp_quality = quality;
grasps_.push_back(grasp);
return 1;
}
static void subject_publish_callback(const ViconDriver::Subject &subject)
{
if(!running)
return;
static std::map<std::string, ros::Publisher> vicon_publishers;
std::map<std::string, ros::Publisher>::iterator it;
it = vicon_publishers.find(subject.name);
if(it == vicon_publishers.end())
{
ros::Publisher pub = nh->advertise<vicon::Subject>(subject.name, 10);
it = vicon_publishers.insert(std::make_pair(subject.name, pub)).first;
}
if(loadCalib(subject.name))
{
Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
current_pose.translate(Eigen::Vector3d(subject.translation));
current_pose.rotate(Eigen::Quaterniond(subject.rotation));
current_pose = current_pose * calib_pose[subject.name];
const Eigen::Vector3d position(current_pose.translation());
const Eigen::Quaterniond rotation(current_pose.rotation());
vicon::Subject subject_ros;
subject_ros.header.seq = subject.frame_number;
subject_ros.header.stamp = ros::Time(subject.time_usec / 1e6);
subject_ros.header.frame_id = "/vicon";
subject_ros.name = subject.name;
subject_ros.occluded = subject.occluded;
subject_ros.position.x = position.x();
subject_ros.position.y = position.y();
subject_ros.position.z = position.z();
subject_ros.orientation.x = rotation.x();
subject_ros.orientation.y = rotation.y();
subject_ros.orientation.z = rotation.z();
subject_ros.orientation.w = rotation.w();
subject_ros.markers.resize(subject.markers.size());
for(size_t i = 0; i < subject_ros.markers.size(); i++)
{
subject_ros.markers[i].name = subject.markers[i].name;
subject_ros.markers[i].subject_name = subject.markers[i].subject_name;
subject_ros.markers[i].position.x = subject.markers[i].translation[0];
subject_ros.markers[i].position.y = subject.markers[i].translation[1];
subject_ros.markers[i].position.z = subject.markers[i].translation[2];
subject_ros.markers[i].occluded = subject.markers[i].occluded;
}
it->second.publish(subject_ros);
}
}
void pose_callback( const nav_msgs::Odometry &pos )
{
now_t = pos.header.stamp;
now_position(0) = pos.pose.pose.position.x;
now_position(1) = pos.pose.pose.position.y;
now_position(2) = pos.pose.pose.position.z;
q_ukf.w() = pos.pose.pose.orientation.w;
q_ukf.x() = pos.pose.pose.orientation.x;
q_ukf.y() = pos.pose.pose.orientation.y;
q_ukf.z() = pos.pose.pose.orientation.z;
now_velocity(0) = pos.twist.twist.linear.x;
now_velocity(1) = pos.twist.twist.linear.y;
now_velocity(2) = pos.twist.twist.linear.z;
rpy = Qbw2RPY(q_ukf);
}
bool planning_models::quatFromMsg(const geometry_msgs::Quaternion &qmsg, Eigen::Quaterniond &q)
{
q = Eigen::Quaterniond(qmsg.w, qmsg.x, qmsg.y, qmsg.z);
double error = fabs(q.squaredNorm() - 1.0);
if (error > 0.05)
{
ROS_ERROR("Quaternion is NOWHERE CLOSE TO NORMALIZED [x,y,z,w], [%.2f, %.2f, %.2f, %.2f]. Can't do much, returning identity.",
qmsg.x, qmsg.y, qmsg.z, qmsg.w);
q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
return false;
}
else if(error > 1e-3)
q.normalize();
return true;
}
inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
#if 0
static const double dt = 0.02;
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
// Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
/// Turn it into an incremental quat to do the space transformation
Eigen::Quaterniond incrementalQuat =
cTb * angVelVecToIncRot(angVel, dt) * cTb.conjugate();
/// Then turn it back into an angular velocity vector
angVel = incRotToAngVelVec(incrementalQuat, dt);
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
#else
kalman::IMUAngVelMeasurement kalmanMeas{angVel, var};
auto correctionInProgress =
kalman::beginUnscentedCorrection(state, kalmanMeas);
auto outputMeas = [&] {
std::cout << "state: " << state.angularVelocity().transpose()
<< " and measurement: " << angVel.transpose();
};
if (!correctionInProgress.stateCorrectionFinite) {
std::cout
<< "Non-finite state correction in applying angular velocity: ";
outputMeas();
std::cout << "\n";
return;
}
if (!correctionInProgress.finishCorrection(true)) {
std::cout << "Non-finite error covariance after applying angular "
"velocity: ";
outputMeas();
std::cout << "\n";
}
#endif
}
TEST(TFEigenConversions, tf_eigen_quaternion)
{
tf::Quaternion t;
t[0] = gen_rand(-1.0,1.0);
t[1] = gen_rand(-1.0,1.0);
t[2] = gen_rand(-1.0,1.0);
t[3] = gen_rand(-1.0,1.0);
t.normalize();
Eigen::Quaterniond k;
quaternionTFToEigen(t,k);
ASSERT_NEAR(t[0],k.coeffs()(0),1e-6);
ASSERT_NEAR(t[1],k.coeffs()(1),1e-6);
ASSERT_NEAR(t[2],k.coeffs()(2),1e-6);
ASSERT_NEAR(t[3],k.coeffs()(3),1e-6);
ASSERT_NEAR(k.norm(),1.0,1e-10);
}
inline Transformation::Transformation(const Eigen::Vector3d & r_AB,
const Eigen::Quaterniond& q_AB)
: r_(¶meters_[0]),
q_(¶meters_[3]) {
r_ = r_AB;
q_ = q_AB.normalized();
updateC();
}
TEST (PCL, WarpPointRigid6DDouble)
{
WarpPointRigid6D<PointXYZ, PointXYZ, double> warp;
Eigen::Quaterniond q (0.4455, 0.9217, 0.3382, 0.3656);
q.normalize ();
Eigen::Vector3d t (0.82550, 0.11697, 0.44864);
Eigen::VectorXd p (6);
p[0] = t.x (); p[1] = t.y (); p[2] = t.z (); p[3] = q.x (); p[4] = q.y (); p[5] = q.z ();
warp.setParam (p);
PointXYZ pin (1, 2, 3), pout;
warp.warpPoint (pin, pout);
EXPECT_NEAR (pout.x, 4.15963, 1e-5);
EXPECT_NEAR (pout.y, -1.51363, 1e-5);
EXPECT_NEAR (pout.z, 0.922648, 1e-5);
}
bool planArmToPose(const string &armName, const Eigen::Vector3d &pos, const Eigen::Quaterniond &q, trajectory_msgs::JointTrajectory &traj, arm_navigation_msgs::SimplePoseConstraint *poseConstraint)
{
clopema_arm_navigation::ClopemaMotionPlan mp;
mp.request.motion_plan_req.group_name = armName + "_arm" ;
mp.request.motion_plan_req.allowed_planning_time = ros::Duration(5.0);
if ( !getRobotState(mp.request.motion_plan_req.start_state) ) return false ;
arm_navigation_msgs::SimplePoseConstraint desired_pose;
desired_pose.header.frame_id = "base_link";
desired_pose.header.stamp = ros::Time::now();
desired_pose.link_name = armName + "_ee";
desired_pose.pose.position.x = pos.x() ;
desired_pose.pose.position.y = pos.y() ;
desired_pose.pose.position.z = pos.z() ;
desired_pose.pose.orientation.x = q.x() ;
desired_pose.pose.orientation.y = q.y() ;
desired_pose.pose.orientation.z = q.z() ;
desired_pose.pose.orientation.w = q.w() ;
if ( poseConstraint )
{
desired_pose.absolute_position_tolerance = poseConstraint->absolute_position_tolerance ;
desired_pose.absolute_roll_tolerance = poseConstraint->absolute_roll_tolerance ;
desired_pose.absolute_pitch_tolerance = poseConstraint->absolute_pitch_tolerance ;
desired_pose.absolute_yaw_tolerance = poseConstraint->absolute_yaw_tolerance ;
}
else
{
desired_pose.absolute_position_tolerance.x = desired_pose.absolute_position_tolerance.y = desired_pose.absolute_position_tolerance.z = 0.02;
desired_pose.absolute_roll_tolerance = desired_pose.absolute_pitch_tolerance = desired_pose.absolute_yaw_tolerance = 0.04;
}
poseToClopemaMotionPlan(mp, desired_pose);
if (!plan(mp)) return false;
traj = mp.response.joint_trajectory ;
if ( traj.points.size() > 2 ) return true ;
return false;
}