41 N_ =
scene_->GetKinematicTree().GetNumControlledJoints();
59 I_ = Eigen::MatrixXd::Identity(
N_,
N_);
65 if (joint_state.rows() !=
N_)
ThrowNamed(
"Wrong size for joint_state!");
87 if (jacobian.rows() !=
N_ || jacobian.cols() !=
N_)
ThrowNamed(
"Wrong size of jacobian! " <<
N_);
Time-derivative estimation by backward differencing. JointVelocityBackwardDifference uses backward di...
double backward_difference_params_
Binomial coefficient parameters.
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void AssignScene(ScenePtr scene) override
std::shared_ptr< Scene > ScenePtr
int N_
Number of dofs for robot.
Eigen::MatrixXd I_
Identity matrix.
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) 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++...
JointVelocityBackwardDifferenceInitializer parameters_
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
REGISTER_TASKMAP_TYPE("JointVelocityBackwardDifference", exotica::JointVelocityBackwardDifference)
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
Eigen::VectorXd q_
Log of previous joint state.
Eigen::VectorXd qbd_
x+qbd_ is a simplified estimate of the first time derivative.