-
Notifications
You must be signed in to change notification settings - Fork 307
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
Finding feasible EE position with custom cost term #185
Comments
Dear @awa152 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Hi,
my goal is to find an endeffector position, which is feasible and as close as possible to the target position.
I am wondering, if the results I get are due to a wrong implementation of my cost term or if it is due to the non-linearity of the forwards-kinematics or even solver specific. If I at least could exclude the first, that would help.
So basically I am trying to solve the inverse kinematics problem using the forwards kinematics equation to find a feasible EE position within the workspace of the manipulator. If the target given is not within the workspace, it should return the closest feasible position (probably not as straightforward as I thought it could be).
As I later on want to use the solution within my MPC, I added extra MPC control variables that represent the position of my joints and are constrained on the limits of said joints. I used your cost function term https://github.com/ethz-adrl/control-toolbox/blob/v3.0.2/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspaceGeometricJacobian.hpp and changed it to my needs:
Too summarize the code above, the only changes I did to the original file are using
rbdState.jointPositions() = u.template tail<KINEMATICS::NJOINTS>()
, to use the last control variables as joint positions. Also I set the derivatives of the states to zero and moved the code for the derivatives from here to the control derivatives. I think from the implementation side this should be correct?The results can be seen in the short video I attached. The orange marker is the target marker and the green one is the current EE position, computed with the joint positions I get from the control variables. As soon as I click the target marker, the optimal solution is found right away. When I drag the target closer to the workspace border and beyond, the solver does not find a solution. I would like the marker to stay as close as possible to the target, but within the feasible workspace. Sometimes it seems to get stuck in a local minimum, while searching the solution space.
I use HPIPM and my solver settings are:
vid1.mp4
The text was updated successfully, but these errors were encountered: