0

I'm currently working on a project that requires me to solve for a few joint configurations that determine a path my robot will take. The joint configurations have some constraints and costs (such as distance and gaze constraints), but I would like to add a cost for some idea of "smoothness" of the path.

To accomplish this, I wanted to create a BsplineTrajectory_, and then get the strain energy. I don't currently see a way to do this though. The only thing I can think of is to reverse engineer all of the polynomials using a bunch of queries to EvaluateCurve, and then use that to analytically determine the average curvature. This seems a little messy though.

I wanted to know if there is currently a way in Drake for me to compute a cost akin to strain energy easily? Otherwise, I was wondering if Drake could provide a strain_energy function, or if not simply a function to get the coefficients of the polynomials?

Zev Minsky-Primus
  • 35
  • 5
  • A b-spline is parameterized by its knots and its control points. The BsplineBasis method (which I think you've found, since you mentioned `EvaluateCurve`) gives access to common computations on that parameterization. Do you have an equation for the strain energy / curvature in terms of the bspline parameters that you want to implement? – Russ Tedrake Sep 03 '22 at 18:52
  • I got my definition from this [paper](https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.47.9671&rep=rep1&type=pdf). It says: A (spline) curve is fair if it minimizes the integral of the squared curvature with respect to the arc length (i.e. strain energy of a thin elastic beam); The equation I have for it is thus `(∫κ^2)/l` where `κ` is curvature and is thus defined by `||f'(t)×f''(t)||/(||f'(t)||^3)`, and `l` is arc length and thus defined by `∫(1 + ||f'(x)||^2)^0.5`. – Zev Minsky-Primus Sep 06 '22 at 15:24
  • that seems reasonable, but there is some work to do yet to interpret those terms in terms of the bspline knots/control points. Do you have an equation for that? – Russ Tedrake Sep 07 '22 at 01:41

0 Answers0