41 N_ =
scene_->GetKinematicTree().GetNumControlledJoints();
54 for (
int i = 0; i < 3; ++i)
66 I_ = Eigen::MatrixXd::Identity(
N_,
N_);
72 if (joint_state.size() !=
N_)
ThrowNamed(
"Wrong size for joint_state!");
75 q_.col(2) =
q_.col(1);
76 q_.col(1) =
q_.col(0);
77 q_.col(0) = joint_state;
95 if (jacobian.rows() !=
N_ || jacobian.cols() !=
N_)
ThrowNamed(
"Wrong size of jacobian! " <<
N_);
Eigen::MatrixXd q_
Log of previous three joint states.
Time-derivative estimation by backward differencing. JointJerkBackwardDifference uses backward differ...
Eigen::Ref< Eigen::VectorXd > VectorXdRef
std::shared_ptr< Scene > ScenePtr
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
int N_
Number of dofs for robot.
Eigen::MatrixXd I_
Identity matrix.
Eigen::VectorXd qbd_
x+qbd_ is a simplifed estimate of the third time derivative.
JointJerkBackwardDifferenceInitializer parameters_
Eigen::Vector3d backward_difference_params_
Binomial cooeficient parameters.
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
REGISTER_TASKMAP_TYPE("JointJerkBackwardDifference", exotica::JointJerkBackwardDifference)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void AssignScene(ScenePtr scene) override
int TaskSpaceDim() override
void SetPreviousJointState(Eigen::VectorXdRefConst joint_state)
Logs previous joint state. SetPreviousJointState must be called after solve is called in a Python/C++...