41 for (
int i = 0; i <
n_effs_; ++i)
44 const int eff_id = 3 * i;
47 phi.segment(eff_id, 3) = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data) -
eff_upper_.segment<3>(eff_id);
59 for (
int i = 0; i <
n_effs_; ++i)
62 const int eff_id = 3 * i;
65 phi.segment(eff_id, 3) = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data) -
eff_upper_.segment<3>(eff_id);
67 jacobian.middleRows(eff_id, 3) =
kinematics[0].jacobian(i).data.topRows<3>();
76 if (eff_id < 0 || eff_id >=
n_effs_)
ThrowNamed(
"Given eff_id (" << eff_id <<
") is out of range [0, " <<
n_effs_ <<
")!");
82 if (eff_id < 0 || eff_id >=
n_effs_)
ThrowNamed(
"Given eff_id (" << eff_id <<
") is out of range [0, " <<
n_effs_ <<
")!");
89 visualization_msgs::MarkerArray ma;
91 for (
int i = 0; i <
n_effs_; ++i)
93 const int eff_id = 3 * i;
94 visualization_msgs::Marker m;
96 std::string frame_name;
97 if (
frames_[i].frame_B_link_name ==
"")
99 frame_name =
scene_->GetRootFrameName();
103 frame_name =
frames_[i].frame_B_link_name;
105 m.header.frame_id =
"exotica/" + frame_name;
107 m.action = visualization_msgs::Marker::ADD;
108 m.type = visualization_msgs::Marker::CUBE;
115 m.pose.orientation.w = 1.0;
118 ma.markers.emplace_back(m);
133 for (
int i = 0; i <
n_effs_; ++i)
135 const int eff_id = 3 * i;
138 FrameWithBoxLimitsInitializer frame(
parameters_.EndEffector[i]);
139 if (frame.XLim[0] > frame.XLim[1])
ThrowPretty(
"Specify XLim using lower then upper for end-effector " << i <<
".");
140 if (frame.YLim[0] > frame.YLim[1])
ThrowPretty(
"Specify YLim using lower then upper for end-effector " << i <<
".");
141 if (frame.ZLim[0] > frame.ZLim[1])
ThrowPretty(
"Specify ZLim using lower then upper for end-effector " << i <<
".");
155 pub_markers_ = Server::Advertise<visualization_msgs::MarkerArray>(
"eff_box_objects", 1,
true);
156 visualization_msgs::Marker md;
158 visualization_msgs::MarkerArray ma;
159 ma.markers.reserve(1);
160 ma.markers.emplace_back(md);
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
void publish(const boost::shared_ptr< M > &message) const
std::vector< KinematicFrameRequest > frames_
int TaskSpaceDim() override
REGISTER_TASKMAP_TYPE("EffBox", exotica::EffBox)
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void Instantiate(const EffBoxInitializer &init) override
void PublishObjectsAsMarkerArray()
Eigen::VectorXd eff_lower_
End-effector lower x, y, z limit.
Eigen::VectorXd eff_upper_
End-effector upper x, y, z limit.
Eigen::Vector3d GetUpperLimit(const int eff_id) const
EffBoxInitializer parameters_
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
Eigen::Vector3d GetLowerLimit(const int eff_id) const
ros::Publisher pub_markers_
Three multiplied by the number of end-effectors.
Limits every given end-effector motion to a box in some reference frame.
int n_effs_
Number of end-effectors.