search

ethz-adrl / towr

A light-weight, Eigen-based C++ library for trajectory optimization for legged robots.
http://wiki.ros.org/towr
BSD 3-Clause "New" or "Revised" License
929 stars 233 forks source link

A small bug in HeightMap::GetDerivativeOfNormalizedBasisWrt() #96

Open caoweiquan322 opened 2 years ago

caoweiquan322 commented 2 years ago

Hello,

I believe I found a bug in HeightMap::GetDerivativeOfNormalizedBasisWrt().

The code below

Vector3d dn_norm_wrt_n = GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex(v, dim);
return dn_norm_wrt_n.cwiseProduct(dv_wrt_dim);

should be replaced by

  Eigen::Matrix3d dn_norm_wrt_n;
  dn_norm_wrt_n << GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex(v, 0),
                   GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex(v, 1),
                   GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex(v, 2);
  return dn_norm_wrt_n * dv_wrt_dim;

Am I correct? Please confirm. Thanks.