44 phi(i) = 1.0 - 2.0 * frac + frac * frac;
62 Eigen::Vector2d o = Eigen::Map<Eigen::Vector2d>(
kinematics[0].Phi(i).p.data);
66 phi(i) = 1.0 - 2.0 * frac + frac * frac;
67 for (
int j = 0; j < jacobian.cols(); ++j) jacobian(i, j) = -4.0 * (1.0 - frac) *
kinematics[0].jacobian[i].data.topRows<2>().col(j).dot(o) /
radii_squared_(i);
72 jacobian.row(i).setZero();
81 Eigen::Vector2d o = Eigen::Map<Eigen::Vector2d>(
kinematics[0].Phi(i).p.data);
82 const double d = o.norm();
84 for (
int j = 0; j < jacobian.cols(); ++j) jacobian(i, j) = -2.0 *
kinematics[0].jacobian[i].data.topRows<2>().col(j).dot(o);
91 visualization_msgs::MarkerArray ma;
95 visualization_msgs::Marker m;
97 m.header.frame_id =
"exotica/" +
frames_[i].frame_B_link_name;
99 m.action = visualization_msgs::Marker::ADD;
100 m.type = visualization_msgs::Marker::SPHERE;
105 m.pose.position.x =
kinematics[0].Phi(i).p.data[0];
106 m.pose.position.y =
kinematics[0].Phi(i).p.data[1];
107 m.pose.position.z =
kinematics[0].Phi(i).p.data[2];
108 m.pose.orientation.x = q[0];
109 m.pose.orientation.y = q[1];
110 m.pose.orientation.z = q[2];
111 m.pose.orientation.w = q[3];
114 ma.markers.emplace_back(m);
140 SphereInitializer sphere(init.EndEffector[i]);
158 pub_markers_ = Server::Advertise<visualization_msgs::MarkerArray>(
"avoid_look_at_sphere_objects", 1,
true);
159 visualization_msgs::Marker md;
160 md.action = visualization_msgs::Marker::DELETEALL;
161 visualization_msgs::MarkerArray ma;
162 ma.markers.push_back(md);
void(AvoidLookAtSphere::* UpdateTaskMapWithJacobian)(Eigen::VectorXdRefConst, Eigen::VectorXdRef, Eigen::MatrixXdRef)
void publish(const boost::shared_ptr< M > &message) const
std::vector< KinematicFrameRequest > frames_
int TaskSpaceDim() override
void UpdateAsInequalityConstraintWithJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
Eigen::VectorXd radii_squared_
The square of each object radii.
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void Instantiate(const AvoidLookAtSphereInitializer &init) override
void UpdateAsCostWithJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
void PublishObjectsAsMarkerArray()
REGISTER_TASKMAP_TYPE("AvoidLookAtSphere", exotica::AvoidLookAtSphere)
void UpdateAsCostWithoutJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
AvoidLookAtSphereInitializer parameters_
ros::Publisher pub_markers_
publish marker for RViz
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void UpdateAsInequalityConstraintWithoutJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
Avoids pointing end-effector at a given spherical object.
Eigen::VectorXd diameter_
The diameter of each object.
int n_objects_
Number of spherical objects.
Eigen::VectorXd GetRotationAsVector(const KDL::Frame &val, RotationType type)
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
void(AvoidLookAtSphere::* UpdateTaskMapWithoutJacobian)(Eigen::VectorXdRefConst, Eigen::VectorXdRef)