HInfinityRobustController::HInfinityRobustController( const DQ_kinematics& robot, const Matrix<double,8,1>& B, const double& gamma, const double& alpha ) : DQ_controller()
{
//Initialization of argument parameters
robot_dofs_ = (robot.links() - robot.n_dummy());
robot_ = robot;
kp_ = MatrixXd::Zero(8,8);
B_ = B;
Bw_ = Matrix<double,8,1>::Zero();
gamma_ = gamma;
alpha_ = alpha;
//Initilization of remaining parameters
thetas_ = MatrixXd(robot_dofs_,1);
delta_thetas_ = MatrixXd::Zero(robot_dofs_,1);
old_reference_ = DQ(0.0);
reference_state_variables_ = MatrixXd(8,1);
measured_state_variables_ = MatrixXd(8,1);
N_ = MatrixXd(8,robot_dofs_);
task_jacobian_ = MatrixXd(8,robot_dofs_);
N_pseudoinverse_ = MatrixXd(robot_dofs_,8);
error_ = MatrixXd(8,1);
C8_ = C8();
identity8_ = MatrixXd::Identity(8,8);
end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);
}
DampedNumericalFilteredController::DampedNumericalFilteredController(DQ_kinematics robot, MatrixXd kp, double beta, double lambda_max, double epsilon) : DQ_controller()
{
//Constants
dq_one_ = DQ(1);
//Initialization of argument parameters
robot_dofs_ = (robot.links() - robot.n_dummy());
robot_ = robot;
kp_ = kp;
ki_ = MatrixXd::Zero(kp.rows(),kp.cols());
kd_ = MatrixXd::Zero(kp.rows(),kp.cols());
beta_ = beta;
lambda_max_ = lambda_max;
//Initilization of remaining parameters
thetas_ = MatrixXd(robot_dofs_,1);
delta_thetas_ = MatrixXd::Zero(robot_dofs_,1);
task_jacobian_ = MatrixXd(8,robot_dofs_);
svd_ = JacobiSVD<MatrixXd>(robot_dofs_,8);
svd_sigma_inverted_ = MatrixXd::Zero(robot_dofs_,8);
identity_ = Matrix<double,8,8>::Identity();
error_ = MatrixXd::Zero(8,1);
integral_error_ = MatrixXd::Zero(8,1);
last_error_ = MatrixXd::Zero(8,1);
at_least_one_error_ = false;
end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);
}
void traverseBFT(int g[GRAPHSIZE][GRAPHSIZE]){
int startingNode;
printf("\n\tBreadth First Traversal:\n\tStarting Node: ");
scanf("%d",&startingNode);
VisitMark mark[GRAPHSIZE]; // to know which nodes to add
List *Q = initQueue();
int i,j;
for (i = 0; i < GRAPHSIZE; i++){
mark[i] = UNVISITED; // For starters, mark all nodes as UNVISITED
}
mark[startingNode] = VISITED;
NQ(&Q,startingNode);
while (!isEmpty(&Q)){
int temp = DQ(&Q);
printf("%d ",temp);
for (j = 0; j < GRAPHSIZE; j++){
if (g[j][temp] == 1){
if (mark[j] == UNVISITED){
mark[j] = VISITED;
NQ(&Q,j);
}
}
}
}
}
void BFS(int i)
{
printf("%d ", i);
reach[i] = 1;
myQueue *root = NULL;
root = NQ(root,i);
node *p = graph[root->value];
while (root != NULL)
{
p = graph[root->value];
root = DQ(root);
while (p != NULL)
{
if (reach[p->vertex] == 0)
{
printf("%d ", p->vertex);
reach[p->vertex] = 1;
root = NQ(root,p->vertex);
}
p = p->next;
}
}
}
int v_accept(int socket, struct in_addr *node){
socket_t *lso = fd_lookup(socket); //lso -> listening socket
socket_t *nso;//nso -> new socket
struct in_addr anyaddr;
anyaddr.s_addr = 0;
if(lso->state != LISTENING) return -1; //"this is not a listening socket"
//get the request
void *request;
DQ(lso->q, &request);
//make an active socket for this connection
int s = v_socket();
v_bind(s, &anyaddr, lso->myport);
nso = fd_lookup(s);
memcpy(&(nso->uraddr), request+TCPHDRSIZE, SIZE32);
memcpy(&(nso->myaddr), request+TCPHDRSIZE+SIZE32, SIZE32);
nso->urport = ((tcphdr *)request)->sourceport;
nso->ackseq= ++(((tcphdr *)request)->seqnum);
nso->myseq = rand() % MAXSEQ;
#ifdef SIMPLESEQ
nso->myseq = 0;
#endif
set_socketstate(nso, SYN_RCVD);
init_windows(nso);
nso->sendw->adwindow = ((tcphdr *)request)->adwindow;
//send respones (second grip)
tcp_send_handshake(2, nso);
//initiate buffer mamagement
pthread_t mgmt_thr;
pthread_attr_t thr_attr;
pthread_attr_init(&thr_attr);
pthread_attr_setdetachstate(&thr_attr, PTHREAD_CREATE_DETACHED);
pthread_create(&mgmt_thr, &thr_attr, buf_mgmt ,(void *) s);
//add to socket_table
HASH_ADD(hh2, socket_table, urport, keylen, nso);
free(request);
//if caller wants request origin
if(node!=NULL) node->s_addr = nso->uraddr;
return s;
}
Pid_SRIvar_TrackingController::Pid_SRIvar_TrackingController( const DQ_kinematics& robot, VectorXd upper_joint_limits, VectorXd lower_joint_limits, const MatrixXd& feedback_gain, const MatrixXd& ki, const double ki_memory, const MatrixXd& kd, const double& beta, const double& lambda_max, const double& epsilon) : DQ_controller()
{
//Constants
dq_one_ = DQ(1);
//Initialization of argument parameters
robot_dofs_ = (robot.links() - robot.n_dummy());
robot_ = robot;
kp_ = feedback_gain;
ki_ = ki;
kd_ = kd;
ki_memory_ = ki_memory;
beta_ = beta;
lambda_max_ = lambda_max;
//Initilization of remaining parameters
thetas_ = MatrixXd(robot_dofs_,1);
delta_thetas_ = MatrixXd::Zero(robot_dofs_,1);
task_jacobian_ = MatrixXd(8,robot_dofs_);
task_jacobian_pseudoinverse_ = MatrixXd(robot_dofs_,8);
svd_ = JacobiSVD<MatrixXd>(robot_dofs_,8);
svd_sigma_inverted_ = MatrixXd::Zero(robot_dofs_,8);
identity_ = Matrix<double,8,8>::Identity();
error_ = MatrixXd(8,1);
integral_error_ = MatrixXd::Zero(8,1);
last_error_ = MatrixXd::Zero(8,1);
at_least_one_error_ = false;
end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);
upper_joint_limits_ = upper_joint_limits;
lower_joint_limits_ = lower_joint_limits;
original_dummy_joints_ = robot_.dummy();
}
void traversePrim(int g[GRAPHSIZE][GRAPHSIZE]){
int startingNode;
printf("\n\tBreadth First Traversal:\n\tStarting Node: ");
scanf("%d",&startingNode);
int i,j;
List *Queue = initQueue();
VisitMark mark[GRAPHSIZE];
for (i = 0; i < GRAPHSIZE; i++){
mark[i] = UNVISITED;
}
int localMin;
int closestNode;
NQ(&Queue,startingNode);
mark[startingNode] = VISITED;
while (!isEmpty(&Queue)){
int temp = DQ(&Queue);
printf("%d ",temp);
localMin = 9999;
closestNode = temp;
for (j = 0; j < GRAPHSIZE; j++){
if (g[j][temp] > 0 && mark[j] == UNVISITED){
if (g[j][temp] < localMin){
localMin = g[j][temp];
closestNode = j;
}
}
}
if (closestNode != temp){
mark[closestNode] = VISITED;
NQ(&Queue,closestNode);
}
}
}
int main()
{
struct Queue *Q = inializeQ();
NQ(Q,1);
NQ(Q,2);
NQ(Q,3);
NQ(Q,4);
NQ(Q,5);
NQ(Q,6);
printQ(Q);
printf("\n[\t");
while(!isEmpty(Q))
{
int data = DQ(&Q);
printf("%d\t", data);
}
printf("]\t\n");
return 0;
}
/*
* Prim's approximated TSP tour
* See also [Cristophides'92]
*/
static
int findEulerianPath(TSP *tsp) {
int *mst, *arc;
int i, j, k, l, a;
int n, *iorder, *jorder;
DTYPE d;
DTYPE maxd;
DTYPE *dist;
DTYPE *dis;
jorder = tsp->jorder;
iorder = tsp->iorder;
dist = tsp->dist;
maxd = tsp->maxd;
n = tsp->n;
if (!(mst =(int*) palloc((size_t) n * sizeof(int))) ||
!(arc =(int*) palloc((size_t) n * sizeof(int))) ||
!(dis =(DTYPE*) palloc((size_t) n * sizeof(DTYPE))) ) {
elog(ERROR, "Failed to allocate memory!");
return -1;
}
// PGR_DBG("findEulerianPath: 1");
k = -1;
j = -1;
d = maxd;
dis[0] = -1;
for (i = 1; i < n; i++) {
dis[i] = D(i, 0);
arc[i] = 0;
if (d > dis[i]) {
d = dis[i];
j = i;
}
}
// PGR_DBG("findEulerianPath: j=%d", j);
if (j == -1)
elog(ERROR, "Error TSP fail to findEulerianPath, check your distance matrix is valid.");
/*
* O(n^2) Minimum Spanning Trees by Prim and Jarnick
* for graphs with adjacency matrix.
*/
for (a = 0; a < n - 1; a++) {
mst[a] = j * n + arc[j]; /* join fragment j with MST */
dis[j] = -1;
d = maxd;
for (i = 0; i < n; i++) {
if (dis[i] >= 0) {
/* not connected yet */
if (dis[i] > D(i, j)) {
dis[i] = D(i, j);
arc[i] = j;
}
if (d > dis[i]) {
d = dis[i];
k = i;
}
}
}
j = k;
}
// PGR_DBG("findEulerianPath: 3");
/*
* Preorder Tour of MST
*/
#define VISITED(x) jorder[x]
#define NQ(x) arc[l++] = x
#define DQ() arc[--l]
#define EMPTY (l == 0)
for (i = 0; i < n; i++) VISITED(i) = 0;
k = 0; l = 0; d = 0; NQ(0);
while (!EMPTY) {
i = DQ();
if (!VISITED(i)) {
iorder[k++] = i;
VISITED(i) = 1;
for (j = 0; j < n - 1; j++) /* push all kids of i */ {
if (i == mst[j]%n) NQ(mst[j]/n);
}
}
}
// PGR_DBG("findEulerianPath: 4");
return 0;
}
/*
* Prim's approximated TSP tour
* See also [Cristophides'92]
*/
bool
TSP::findEulerianPath() {
Ids iorder(n);
Ids mst(n);
Ids arc(n);
std::vector < double > dis(n);
double d;
#if 0
int n, *iorder, *jorder;
DTYPE d;
DTYPE maxd;
DTYPE *dist;
DTYPE *dis;
jorder = tsp->jorder;
iorder = tsp->iorder;
dist = tsp->dist;
maxd = tsp->maxd;
n = tsp->n;
if (!(mst = (int*) palloc(n * sizeof(int))) ||
!(arc = (int*) palloc(n * sizeof(int))) ||
!(dis = (DTYPE*) palloc(n * sizeof(DTYPE))) )
{
elog(ERROR, "Failed to allocate memory!");
return -1;
}
#endif
// PGR_DBG("findEulerianPath: 1");
size_t j(0);
double curr_maxd = maxd;
dis[0] = -1;
for (size_t i = 1; i < n; ++i) {
dis[i] = dist[i][0];
arc[i] = 0;
if (curr_maxd > dis[i]) {
curr_maxd = dis[i];
j = i;
}
}
//PGR_DBG("findEulerianPath: j=%d", j);
if (curr_maxd == maxd) {
// PGR_DBG("Error TSP fail to findEulerianPath, check your distance matrix is valid.");
return false;
}
/*
* O(n^2) Minimum Spanning Trees by Prim and Jarnick
* for graphs with adjacency matrix.
*/
for (size_t a = 0; a < n - 1; a++) {
size_t k(0);
mst[a] = j * n + arc[j]; /* join fragment j with MST */
dis[j] = -1;
d = maxd;
for (size_t i = 0; i < n; i++)
{
if (dis[i] >= 0) /* not connected yet */
{
if (dis[i] > dist[i][j])
{
dis[i] = dist[i][j];
arc[i] = j;
}
if (d > dis[i])
{
d = dis[i];
k = i;
}
}
}
j = k;
}
//PGR_DBG("findEulerianPath: 3");
/*
* Preorder Tour of MST
*/
#if 0
#define VISITED(x) jorder[x]
#define NQ(x) arc[l++] = x
#define DQ() arc[--l]
#define EMPTY (l==0)
#endif
for (auto &val : jorder) {
val = 0;
}
#if 0
for (i = 0; i < n; i++) VISITED(i) = 0;
#endif
size_t l = 0;
size_t k = 0;
d = 0;
arc[l++] = 0;
while (!(l == 0)) {
size_t i = arc[--l];
if (!jorder[i]) {
iorder[k++] = i;
jorder[i] = 1;
/* push all kids of i */
for (size_t j = 0; j < n - 1; j++) {
if (i == mst[j] % n)
arc[l++] = mst[j] % n;
}
}
}
#if 0
k = 0; l = 0; d = 0; NQ(0);
while (!EMPTY)
{
i = DQ();
if (!VISITED(i))
{
iorder[k++] = i;
VISITED(i) = 1;
for (j = 0; j < n - 1; j++) /* push all kids of i */
{
if (i == mst[j]%n) NQ(mst[j]/n);
}
}
}
#endif
//PGR_DBG("findEulerianPath: 4");
update(iorder);
return true;
}
