Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement joint position limits #22

Merged
merged 4 commits into from
Sep 28, 2022
Merged

Implement joint position limits #22

merged 4 commits into from
Sep 28, 2022

Conversation

diegoferigo
Copy link
Member

@diegoferigo diegoferigo commented Sep 27, 2022

This PR implements joint position limits by reading the following fields from a parsed SDF model:

Despite we read both parameters, the model used by the physics just uses the dissipation parameter as $k_{limit}$. This is because the stiffness/dissipation parameters could be mapped respectively to a spring/damper model, and the implemented model is equivalent to a pure spring. This means that the joint velocity is not used, and the torque is computed just as a term proportional to the joint position error wrt the limit.

I've tried also two different spring/damper models:

$$ \tau_{limit}^{(1)} = \begin{cases} k_{damper} (s_{min} - s) + k_{spring} \dot{s}, \quad \text{if } s < s_{min} \\ k_{damper} (s_{max} - s) - k_{spring} \dot{s}, \quad \text{if } s > s_{max} \\ \end{cases} $$

and

$$\tau_{limit}^{(2)} = \begin{cases} k_{damper} (s_{min} - s) + k_{spring} \text{abs}(s_{min} - s) \dot{s}, \quad \text{if } s < s_{min} \\\ k_{damper} (s_{max} - s) - k_{spring} \text{abs}(s_{max} - s) \dot{s}, \quad \text{if } s > s_{max} \end{cases}$$

Beyond requiring to tune an additional $k_{spring}$ parameter, both models do not give interesting additions. Particularly, $\tau_{limit}^{(1)}$ produces very often NaNs due to the discontinuous behavior (that would also affect differentiability #4). Instead, $\tau_{limit}^{(2)}$ enforces linearity by smoothing the spring gain with the position error, but beyond being having a less physical explanation, it works only with small $k_{spring}$ in order to prevent NaNs.

For the time being, I prefer keeping things simple. More advanced models can be introduced in the future. Note that a more advanced methodology exploits a constraint solver for joint limits, based on the Gauss principle of least constraint. Since it would make the forward step of the simulation much heavier, let's begin supporting the simple model of this PR.

Here below an example with a simple pendulum. GIFs are recorded with a real-time factor of 0.2.

w/o limits w/ limits

Closes #20.

@diegoferigo diegoferigo self-assigned this Sep 27, 2022
@diegoferigo diegoferigo merged commit 2efe5c2 into main Sep 28, 2022
@diegoferigo diegoferigo deleted the feature/joint_limits branch September 28, 2022 15:30
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Enforce joint position limits
2 participants