50 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
63 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
66 jacobian.row(i) =
kinematics[0].jacobian[i].data.middleRows<1>(2);
77 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
80 jacobian.row(i) =
kinematics[0].jacobian[i].data.middleRows<1>(2);
81 hessian(i).block(0, 0, jacobian.cols(), jacobian.cols()) =
kinematics[0].hessian[i](2);
92 visualization_msgs::MarkerArray msg;
94 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
96 visualization_msgs::Marker plane;
98 plane.header.frame_id =
frames_[i].frame_B_link_name ==
"" ?
"exotica/" +
scene_->GetRootFrameName() :
frames_[i].frame_B_link_name;
99 plane.ns =
frames_[i].frame_A_link_name;
101 plane.type = plane.CUBE;
102 plane.action = plane.ADD;
103 plane.frame_locked =
true;
104 plane.color.g = 1.0f;
105 plane.color.a = 0.8f;
108 plane.scale.x = 10.f;
109 plane.scale.y = 10.f;
110 plane.scale.z = 0.01f;
111 plane.pose.position.z -= (plane.scale.z / 2);
113 msg.markers.push_back(plane);
void Instantiate(const PointToPlaneInitializer &init) override
void publish(const boost::shared_ptr< M > &message) const
std::vector< KinematicFrameRequest > frames_
Eigen::Ref< Eigen::VectorXd > VectorXdRef
int TaskSpaceDim() override
void Update(Eigen::VectorXdRefConst q, Eigen::VectorXdRef phi) override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Hessian > HessianRef
REGISTER_TASKMAP_TYPE("PointToPlane", exotica::PointToPlane)
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
ros::Publisher debug_pub_