Commit df42a6a6 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

finish implementatino for ADDTO and RMTO

parent 9f5bf113
......@@ -340,7 +340,7 @@ namespace pinocchio
switch(op)
{
case SETTO:
Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
break;
case ADDTO:
......@@ -608,7 +608,28 @@ namespace pinocchio
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
switch(op)
{
case SETTO:
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
case ADDTO:
Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
case RMTO:
Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
case APPLY_ON_THE_LEFT:
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose() * Jout;
break;
case APPLY_ON_THE_RIGHT:
Jout = Jout * exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -617,7 +638,27 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianOut_t>& J,
const AssignmentOperatorType op)
{
Jexp6(MotionRef<const Tangent_t>(v.derived()), J.derived());
switch(op)
{
case SETTO:
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
case ADDTO:
Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
case RMTO:
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
case APPLY_ON_THE_LEFT:
break;
case APPLY_ON_THE_RIGHT:
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
// interpolate_impl use default implementation.
......
......@@ -198,7 +198,7 @@ namespace pinocchio
default:
assert(false && "Wrong Op requesed value");
break;
}
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -416,7 +416,29 @@ namespace pinocchio
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
Jout = exp3(-v);
switch(op)
{
case SETTO:
Jout = exp3(-v);
break;
case ADDTO:
Jout += exp3(-v);
break;
case RMTO:
Jout -= exp3(-v);
break;
case APPLY_ON_THE_LEFT:
//TODO: Improve implementation
Jout = exp3(-v) * Jout;
break;
case APPLY_ON_THE_RIGHT:
//TODO: Improve implementation
Jout = Jout * exp3(-v);
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -425,7 +447,26 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
Jexp3(v, J.derived());
switch(op)
{
case SETTO:
Jexp3<SETTO>(v, J.derived());
break;
case ADDTO:
Jexp3<ADDTO>(v, J.derived());
break;
case RMTO:
Jexp3<RMTO>(v, J.derived());
break;
case APPLY_ON_THE_LEFT:
break;
case APPLY_ON_THE_RIGHT:
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment