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()); }