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