search

hungpham2511 / toppra

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

First Order Constraints #20

Closed EdsterG closed 5 years ago

EdsterG commented 5 years ago

The paper mentions first order constraints, however I can't seem to find them in the current implementation. Can you point me in the right direction?

hungpham2511 commented 5 years ago

The simplest First-Order constraint is a JointVelocityConstraint. Suppose you have something like

A(q) qdot <= q

which can be written as

A(q) q_s(s) sdot <= q,

By using A(q) q_s(s) in placed of qs in the below line https://github.com/hungpham2511/toppra/blob/8673344e672de44ebe1305f7995c0c4eea7df01d/toppra/constraint/joint_velocity.py#L7 you can handle any first-order constraint.

EdsterG commented 5 years ago

I'm trying to implement task space velocity limits, however it doesn't seem to be working. Here's what I have so far: https://gist.github.com/EdsterG/280262de0b5c8cd91331fc331c83ccec

More specifically, the duration should not be 400+ seconds. It should actually be less than 30.

hungpham2511 commented 5 years ago

The problem is in line 26, instead of 

xbound = np.asarray(xbound_)

you should use 

xbound = np.array(xbound_)

The reason is that asarray makes xbound a reference to xbound_, while it should be a copy.

Other than that things look quite alright.

EdsterG commented 5 years ago

Got it, thank you.

hungpham2511 commented 5 years ago

@EdsterG Appreciate if you can submit a PR for this (preferably with some tests).

EdsterG commented 5 years ago

Will do once I have some tests