bool LocallyWeightedRegression::predict(const double xQuery, double &yPrediction) { if (!initialized_) { printf("ERROR: LWR model not initialized.\n"); return initialized_; } double sx = 0; double sxtd = 0; for (int i = 0; i < numRfs_; i++) { double psi = getKernel(xQuery, i); sxtd += psi * thetas_(i) * xQuery; sx += psi; } yPrediction = sxtd / sx; return true; }
VectorXd Pid_SRIvar_TrackingController::getNewJointVelocities( const DQ reference, const VectorXd thetas) { thetas_ = thetas; //This is necessary for the getNewJointPositions to work DQ robot_base = robot_.base(); DQ robot_effector = robot_.effector(); VectorXd pseudo_dummy_joint_marker = VectorXd::Zero(robot_dofs_); VectorXd pseudo_thetas = thetas_; VectorXd pseudo_delta_thetas = VectorXd::Zero(robot_dofs_); VectorXd possible_new_thetas = VectorXd::Zero(robot_dofs_); MatrixXd original_dh_matrix = robot_.getDHMatrix(); MatrixXd step_dh_matrix = original_dh_matrix; DQ_kinematics pseudo_robot(original_dh_matrix); pseudo_robot.set_base(robot_base); pseudo_robot.set_effector(robot_effector); bool should_break_loop = false; while(not should_break_loop) { //Calculate jacobian task_jacobian_ = pseudo_robot.analyticalJacobian(pseudo_thetas); // Recalculation of measured data. end_effector_pose_ = pseudo_robot.fkm(pseudo_thetas); //Error last_error_ = error_; error_ = vec8(reference - end_effector_pose_); integral_error_ = error_ + ki_memory_*integral_error_; //error_ = vec8(dq_one_ - conj(end_effector_pose_)*reference); ///Inverse calculation svd_.compute(task_jacobian_, ComputeFullU); singular_values_ = svd_.singularValues(); //Damping Calculation int current_step_relevant_dof = ( pseudo_robot.links() - pseudo_robot.n_dummy() ); double sigma_min = singular_values_( current_step_relevant_dof - 1); VectorXd u_min = svd_.matrixU().col( current_step_relevant_dof - 1); double lambda = lambda_max_; if (sigma_min < epsilon_) { lambda = (1-(sigma_min/epsilon_)*(sigma_min/epsilon_))*lambda_max_*lambda_max_; } task_jacobian_pseudoinverse_ = (task_jacobian_.transpose())*((task_jacobian_*task_jacobian_.transpose() + (beta_*beta_)*identity_ + (lambda*lambda)*u_min*u_min.transpose()).inverse()); // pseudo_delta_thetas = task_jacobian_pseudoinverse_*kp_*error_; if( at_least_one_error_ ) pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ + kd_*(error_ - last_error_) ); else { at_least_one_error_ = true; pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ ); } //Update delta_thetas for possiblenewthetas calculation for(int i=0,j=0; i < robot_dofs_; i++) { if(pseudo_dummy_joint_marker(i) == 0) { delta_thetas_(i) = pseudo_delta_thetas(j); ++j; } //Do NOTHING if it should be ignored. } //Possible new thetas possible_new_thetas = thetas_ + delta_thetas_; //Verify if loop should end should_break_loop = true; int j=0; //For all joints for(int i = 0; i < robot_dofs_; i++) { //If joint is not yet marked for not being considered in the minimization if(pseudo_dummy_joint_marker(i) == 0) { //If the controller is trying to put a joint further than any of its limits if( possible_new_thetas(i) > upper_joint_limits_(i) || possible_new_thetas(i) < lower_joint_limits_(i) ) { //If the joint was already saturated sometime ago double ep = 1.e-05; if ( thetas_(i) > upper_joint_limits_(i) - ep || thetas_(i) < lower_joint_limits_(i) + ep) { pseudo_dummy_joint_marker(i) = 1; //Mark it to be ignored in the minization //std::cout << std::endl << "Joint " << i << " will be ignored in the next controller step."; step_dh_matrix(4,i) = 1; //Set matrix as dummy. step_dh_matrix(0,i) = thetas_(i); //Set matrix theta as a fixed value. should_break_loop = false; } //If the joint was not yet saturated and the controller wants to saturate it else { // Saturate the joint in this step. if ( possible_new_thetas(i) > upper_joint_limits_(i) ) { delta_thetas_(i) = upper_joint_limits_(i) - thetas_(i); //std::cout << std::endl << "Joint = " << i << " was saturated in its upper_limit"; } else if ( possible_new_thetas(i) < lower_joint_limits_(i) ) { delta_thetas_(i) = lower_joint_limits_(i) - thetas_(i); //std::cout << std::endl << "Joint = " << i << " was saturated in its lower_limit"; } else { std::cout << std::endl << "Something is really wrong"; } //The joint should still be considered in the minimizations. pseudo_thetas(j) = thetas_(i); ++j; } } //If the controller is not trying to put this joint further than any of its limits, we consider the velocity given normally else { delta_thetas_(i) = pseudo_delta_thetas(j); pseudo_thetas(j) = thetas_(i); ++j; } } //If joint was marked to be ignored, it shall be ignored. else { delta_thetas_(i) = 0; } } if( j == 0 ) { std::cout << std::endl << "Robot will be unable to get out of this configuration using this controller."; delta_thetas_ = VectorXd::Zero(robot_dofs_); break; } if(not should_break_loop) { pseudo_robot = DQ_kinematics(step_dh_matrix); //Change DH pseudo_robot.set_base(robot_base); pseudo_robot.set_effector(robot_effector); pseudo_thetas.conservativeResize(j); //Resize pseudothetas } }//While not should_break_loop return delta_thetas_; }