You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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).
virtualdoubleevaluate(const Eigen::Matrix<double, STATE_DIM, 1>& x, const Eigen::Matrix<double, CONTROL_DIM, 1>& u,
constdouble& t) override
{
// transform the robot state vector into a CT RBDState
RBDState rbdState;
rbdState.jointPositions() = u.templatetail<KINEMATICS::NJOINTS>(); // x.template head<KINEMATICS::NJOINTS>();// position difference in world frame
Eigen::Matrix<double, 3, 1> xCurr =
kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions()).toImplementation();
Eigen::Matrix<double, 3, 1> xDiff = xCurr - x_ref_;
// compute the cost from the position errordouble pos_cost = 0.5 * (xDiff.transpose() * Q_pos_ * xDiff)(0, 0);
return pos_cost;
}
//! compute derivative of this cost term w.r.t. the statevirtual core::StateVector<STATE_DIM> stateDerivative(const core::StateVector<STATE_DIM>& x,
const core::ControlVector<CONTROL_DIM>& u,
constdouble& t) override
{
return core::StateVector<STATE_DIM>::Zero();
}
virtual core::ControlVector<CONTROL_DIM> controlDerivative(const core::StateVector<STATE_DIM>& x,
const core::ControlVector<CONTROL_DIM>& u,
constdouble& t) override
{
core::StateVector<CONTROL_DIM> grad;
grad.setZero();
RBDState rbdState;
rbdState.jointPositions() = u.templatetail<KINEMATICS::NJOINTS>();
Eigen::Matrix<double, 6, KINEMATICS::NJOINTS> J = kinematics_.getJacobianBaseEEbyId(eeInd_, rbdState);
Eigen::Matrix<double, 3, KINEMATICS::NJOINTS> J_pos = J.templatebottomRows<3>();
// position difference in world frame
Eigen::Matrix<double, 3, 1> xCurr =
kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions()).toImplementation();
Eigen::Matrix<double, 3, 1> xDiff = xCurr - x_ref_;
grad.segment(KINEMATICS::NJOINTS, (KINEMATICS::NJOINTS * 2) - 1) += J_pos.transpose() * Q_pos_ * xDiff;
return grad;
}
virtualstate_matrix_tstateSecondDerivative(const core::StateVector<STATE_DIM>& x,
const core::ControlVector<CONTROL_DIM>& u, constdouble& t) override
{
returnstate_matrix_t::Zero();
}
virtualcontrol_matrix_tcontrolSecondDerivative(const core::StateVector<STATE_DIM>& x,
const core::ControlVector<CONTROL_DIM>& u, constdouble& t) override
{
RBDState rbdState;
rbdState.jointPositions() = u.templatetail<KINEMATICS::NJOINTS>();
Eigen::Matrix<double, 6, KINEMATICS::NJOINTS> J = kinematics_.getJacobianBaseEEbyId(eeInd_, rbdState);
Eigen::Matrix<double, 3, KINEMATICS::NJOINTS> J_pos = J.templatebottomRows<3>();
control_matrix_t Q = control_matrix_t::Zero();
Q.templatebottomRightCorner<KINEMATICS::NJOINTS, KINEMATICS::NJOINTS>() += J_pos.transpose() * Q_pos_ * J_pos;
return Q;
}
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.
Dear @awa152
nice video! I believe in principle we are facing the same problem here and in the other issue you opened.
In principle, the problem is ill conditioned in singularities, you may be seeing this effect here. But without analysing the morphology of your robot, it is hard to say if you are in fact hitting a singularity or not!
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: