search

hungpham2511 / toppra

robotic motion planning library
https://hungpham2511.github.io/toppra/index.html
MIT License
612 stars 169 forks source link

Added CartesianSpeedConstraint #144

Closed stevegolton closed 3 years ago

stevegolton commented 3 years ago

Added CartesianSpeedConstraint to constrain the magnitude of the Cartesian velocity vectors for a part of the robot.

Changes in this PR:

Checklists:

hungpham2511 commented 3 years ago

Hi @stevegolton, many thanks for the PR. I am sorry for not getting back earlier. Was really busy at work in the past weeks. :)

Could you help to make the CI task call cpp-python36? Look like PyKDL was used but not installed.

Update CHANGELOG.md with a single line describing this PR - I was unable to find this file?

It should be HISTORY.md

stevegolton commented 3 years ago

No worries! Likewise, apologies for not getting back to you sooner either. Work was really busy for me too.

I'm struggling a little with the PyKDL dependency issue. I was using this library because I am used to its C++ API, but in hindsight it looks like it isn't well supported and I'm having a bunch of issues getting the python library installed on the cimg/python:3.6 docker container.

I think I might update the example to use pinocchio instead.

hungpham2511 commented 3 years ago

I am sure that you don't need to square the cartesian acceleration. Most likely there is a bug somewhere.

Bests,

Hung

From: Joseph Mirabel notifications@github.com Sent: Saturday, November 14, 2020 7:12:44 PM To: hungpham2511/toppra toppra@noreply.github.com Cc: Hung Pham (Phạm Tiến Hùng) hungpham2511@gmail.com; Comment comment@noreply.github.com Subject: Re: [hungpham2511/toppra] Added CartesianSpeedConstraint (#144)

@jmirabel commented on this pull request.

In toppra/constraint/cartesian_constraints.pyhttps://github.com/hungpham2511/toppra/pull/144#discussion_r523408053:

  • velocity vectors for some the monitored part of the robot.
  • linear_speed_max: float
  • The max linear speed allowed for the monitored part.
  • angular_speed_max: float
  • The max angular speed allowed for the monitored part.
  • dof: int
  • The dimension of the joint position.
  • """
  • super(CartesianSpeedConstraint, self).init(self.invdyn, self.constraintf, self.constraintg, dof)
  • self.fk = fk
  • self.linear_speed_max = linear_speed_max
  • self.angular_speed_max = angular_speed_max
  • def invdyn(self, q, dq, ddq):
  • linear_speed, angular_speed = self.fk(q, dq)
  • return np.array([linear_speed2, angular_speed2])

I think you have to square it for the reason I mentioned in #101https://github.com/hungpham2511/toppra/issues/101. Following the derivation of #146https://github.com/hungpham2511/toppra/pull/146, you should be able to understand why.

— You are receiving this because you commented. Reply to this email directly, view it on GitHubhttps://github.com/hungpham2511/toppra/pull/144#discussion_r523408053, or unsubscribehttps://github.com/notifications/unsubscribe-auth/ACKD6TG6URP2D7IOV5EGWR3SPZQ2ZANCNFSM4SMOOT3Q.

hungpham2511 commented 3 years ago

Ah, the constraints are on Cartesian velocity! That's why. The way that you implement it only work if the constraints are second order but velocity constraints are first-order.

Bests,

Hung

From: Hung Pham hungpham2511@gmail.com Sent: Saturday, November 14, 2020 9:28:19 PM To: hungpham2511/toppra toppra@noreply.github.com; hungpham2511/toppra reply@reply.github.com Cc: Comment comment@noreply.github.com Subject: Re: [hungpham2511/toppra] Added CartesianSpeedConstraint (#144)

I am sure that you don't need to square the cartesian acceleration. Most likely there is a bug somewhere.

Bests,

Hung

From: Joseph Mirabel notifications@github.com Sent: Saturday, November 14, 2020 7:12:44 PM To: hungpham2511/toppra toppra@noreply.github.com Cc: Hung Pham (Phạm Tiến Hùng) hungpham2511@gmail.com; Comment comment@noreply.github.com Subject: Re: [hungpham2511/toppra] Added CartesianSpeedConstraint (#144)

@jmirabel commented on this pull request.

In toppra/constraint/cartesian_constraints.pyhttps://github.com/hungpham2511/toppra/pull/144#discussion_r523408053:

  • velocity vectors for some the monitored part of the robot.
  • linear_speed_max: float
  • The max linear speed allowed for the monitored part.
  • angular_speed_max: float
  • The max angular speed allowed for the monitored part.
  • dof: int
  • The dimension of the joint position.
  • """
  • super(CartesianSpeedConstraint, self).init(self.invdyn, self.constraintf, self.constraintg, dof)
  • self.fk = fk
  • self.linear_speed_max = linear_speed_max
  • self.angular_speed_max = angular_speed_max
  • def invdyn(self, q, dq, ddq):
  • linear_speed, angular_speed = self.fk(q, dq)
  • return np.array([linear_speed2, angular_speed2])

I think you have to square it for the reason I mentioned in #101https://github.com/hungpham2511/toppra/issues/101. Following the derivation of #146https://github.com/hungpham2511/toppra/pull/146, you should be able to understand why.

— You are receiving this because you commented. Reply to this email directly, view it on GitHubhttps://github.com/hungpham2511/toppra/pull/144#discussion_r523408053, or unsubscribehttps://github.com/notifications/unsubscribe-auth/ACKD6TG6URP2D7IOV5EGWR3SPZQ2ZANCNFSM4SMOOT3Q.

stevegolton commented 3 years ago

Looks like @jmirabel's PR #146 does everything this PR does and more (c++ interface & variable velocity limits) which is great! Thus, there's no need for this PR anymore so I'll close it.

mrunaljsarvaiya commented 2 years ago

@stevegolton Did you land up using the cpp-binding to use the cartesian constraints in python or write a pure python constraint using the cpp code as a reference?

stevegolton commented 2 years ago

@mrunaljsarvaiya Hey, I ended up using the cpp version natively in the end. I haven't touched the python versions or any of the bindings since this PR.