CartPoseErrCalculator(const OR::Transform& pose, RobotAndDOFPtr manip, OR::KinBody::LinkPtr link) : pose_inv_(pose.inverse()), manip_(manip), link_(link) {}
CartPoseErrCalculator(const OR::Transform& pose, ConfigurationPtr manip, OR::KinBody::LinkPtr link,Eigen::Vector3d offset) : pose_inv_(pose.inverse()), manip_(manip), link_(link), offset_(offset){}