Commit 3d07a294 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Use overloaded motionAction

parent 5d539a09
// //
// Copyright (c) 2017 CNRS // Copyright (c) 2017-2018 CNRS
// //
// This file is part of Pinocchio // This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it // Pinocchio is free software: you can redistribute it
...@@ -271,7 +271,7 @@ namespace se3 ...@@ -271,7 +271,7 @@ namespace se3
vtmp = -data.v[jointId]; vtmp = -data.v[jointId];
motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols); motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols);
a_partial_dv_cols += oMlast.inverse().toActionMatrix() * dJcols; // TODO: optimize computations motionSet::se3ActionInverse<ADDTO>(oMlast,dJcols,a_partial_dv_cols);
} }
// dacc/dq // dacc/dq
...@@ -284,7 +284,7 @@ namespace se3 ...@@ -284,7 +284,7 @@ namespace se3
motionSet::motionAction(atmp,Jcols,a_partial_dq_cols); motionSet::motionAction(atmp,Jcols,a_partial_dq_cols);
if(parent >0) if(parent >0)
a_partial_dq_cols += vtmp.toActionMatrix() * dJcols; motionSet::motionAction<ADDTO>(vtmp,dJcols,a_partial_dq_cols);
} }
else else
{ {
...@@ -294,7 +294,7 @@ namespace se3 ...@@ -294,7 +294,7 @@ namespace se3
motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols); motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols);
} }
a_partial_dq_cols += vtmp.toActionMatrix() * v_partial_dq_cols; // TODO: optimize computations motionSet::motionAction<ADDTO>(vtmp,v_partial_dq_cols,a_partial_dq_cols);
} }
......
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