Closed EdsterG closed 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.
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.
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.
Got it, thank you.
@EdsterG Appreciate if you can submit a PR for this (preferably with some tests).
Will do once I have some tests
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?