void CdsWrapper::init(CdsWrapperParams params, const KDL::JntArray& q_init)
{
arm_.init(params.arm_params_);
cds_controller_.setObjectFrame(toMathLib(params.cds_params_.object_frame_));
cds_controller_.setAttractorFrame(toMathLib(params.cds_params_.attractor_frame_));
cds_controller_.setDT(params.cds_params_.dt_);
cds_controller_.setCurrentEEPose(toMathLib(arm_.get_pos_fk(q_init)));
cds_controller_.init(static_cast<GMRDynamics*>(params.cds_params_.master_dyn_),
static_cast<GMRDynamics*>(params.cds_params_.slave_dyn_),
static_cast<GMR*>(params.cds_params_.coupling_));
cds_controller_.setMotionParameters(params.cds_params_.alpha_,
params.cds_params_.beta_, params.cds_params_.lambda_,
params.cds_params_.reachingThreshold_);
}
void CdsCartesianWrapper::init(CDSExecutionParams params,
const KDL::Frame& pose_init, int segment_id)
{
// cds_controller_.init(static_cast<GMRDynamics*>(params.master_dyn_),
// static_cast<GMRDynamics*>(params.slave_dyn_),
// static_cast<GMR*>(params.coupling_));
cds_controller_.initSimple(segment_id);
cds_controller_.setObjectFrame(toMathLib(params.object_frame_));
cds_controller_.setAttractorFrame(toMathLib(params.attractor_frame_));
cds_controller_.setCurrentEEPose(toMathLib(pose_init));
cds_controller_.setDT(params.dt_);
cds_controller_.setMotionParameters(params.alpha_,
params.beta_, params.lambda_, params.reachingThreshold_,
params.slave_dynamics_id_);
cds_controller_.postInit();
}
const KDL::JntArray& CdsWrapper::update(const KDL::JntArray& q, double dt,
KDL::Frame& des_pose)
{
assert(q.rows() == arm_.get_dof());
KDL::Frame pose = arm_.get_pos_fk(q);
cds_controller_.setCurrentEEPose(toMathLib(pose));
des_pose = toKDL(cds_controller_.getNextEEPose());
des_pose.M = arm_.get_pos_fk(q).M;
// return arm_.get_vel_ik(q, toKDL(cds_controller_.getNextEEPose()), dt);
return arm_.get_vel_ik(q, des_pose, dt);
}
KDL::Frame CdsCartesianWrapper::update(const KDL::Frame& current_pose)
{
cds_controller_.setCurrentEEPose(toMathLib(current_pose));
return toKDL(cds_controller_.getNextEEPose());
}
