Update robot_models_node.cpp

src/robot_models_node.cpp

@@ -55,54 +55,79 @@
         worldobject o10;
 
 
-        o5a.name = "panda_joint1";
-        o5b.name = "panda_joint2";
-        o5c.name = "panda_joint3";
-        o5d.name = "panda_joint4";
-        o5e.name = "panda_joint5";
-        o5f.name = "panda_joint6";
-        o5g.name = "panda_joint7";
-        o5a.or2 = {1,5,6,7};
-        o5b.or2 = {2,3,5,4};
-        o5c.or2 = {5,1,9,6};
-        o5d.or2 = {6,3,8,9};
-        o5e.or2 = {8,2,7,5};
-        o5f.or2 = {7,4,6,4};
-        o5g.or2 = {9,5,5,8};
-        o5a.p1 = {1, 10, 9};
-        o5b.p1 = {14, 11, 6};
-        o5c.p1 = {4, 13, 7};
-        o5d.p1 = {2, 12, 1};
-        o5e.p1 = {3, 16, 5};
-        o5f.p1 = {11, 15, 3};
-        o5g.p1 = {5, 18, 4};
-
-        o8.robname = "Robot FPE";
-        o8.of = true;
-        o8.moving = false;
-        o8.grasppos = true;
-        o8.planninggrp = "Panda Teaching";
-
-        o3.grippos = {12, 14, 18};
-
-        o4.humannum = 1;
-
-        o2.name = "Cube";
-        o2.size = {5, 7, 9};
-        o2.p1 = {13, 14, 17};
-        o2.x = 2.3;
-        o2.y = 1.2;
-        o2.z = 0.2;
-        o2.w = 0.1;
-
-
-        o6.name = "Ball";
-        o6.size = {1, 2, 3};
-        o6.p1 = {12, 16, 17};
-        o6.x = 1.1;
-        o6.y = 1.4;
-        o6.z = 1.6;
-        o6.w = 1.9;
+        o5a.name = "panda_link1";
+        o5b.name = "panda_link2";
+        o5c.name = "panda_link3";
+        o5d.name = "panda_link4";
+        o5e.name = "panda_link5";
+        o5f.name = "panda_link6";
+        o5g.name = "panda_link7";
+        o5a.or2 = {1.2,5.3,6.5,7.4};
+        o5b.or2 = {2.5,3.6,5.8,4.9};
+        o5c.or2 = {5.1,1.2,9.9,6.8};
+        o5d.or2 = {6.3,3.4,8.5,9.6};
+        o5e.or2 = {8.2,2.6,7.9,5.5};
+        o5f.or2 = {7.3,4.4,6.7,4.0};
+        o5g.or2 = {9.0,5.2,5.5,8.7};
+        o5a.p1 = {1.3, 10.1, 9.4};
+        o5b.p1 = {14.7, 11.3, 6.5};
+        o5c.p1 = {4.9, 13.8, 7.7};
+        o5d.p1 = {2.4, 12.5, 1.6};
+        o5e.p1 = {3.1, 16.2, 5.3};
+        o5f.p1 = {11.5, 15.4, 3.4};
+        o5g.p1 = {5.1, 18.3, 4.2};
+
+        o9.c->robname="Robot FPE";
+        //o8.robname = "Robot FPE";
+        //o8.of = true;
+        o9.c->of=true;
+        o9.c->moving=false;
+        //o8.moving = false;
+        o9.c->grasppos=true;
+        //o8.grasppos = true;
+        //o8.planninggrp = "Panda Teaching";
+        o9.c->planninggrp= "Panda Teaching";
+        //o3.grippos = {12, 14, 18};
+
+        o8.h->grippos= {12.1, 14.3, 18.2};
+
+        o9.a->humannum = 1;
+
+        o9.b1->name="Cube";
+        //o2.name = "Cube";
+        o9.b1->size={5.2, 7.5, 9.1};
+        //o2.size = {5.2, 7.5, 9.1};
+        o9.b1->p1= {13.2, 14.1, 17.5};
+        //o2.p1 = {13.2, 14.1, 17.5};
+
+        // Using association relation quaternion
+        o9.b1->x= 2.3;
+        o9.b1->y = 1.2;
+        o9.b1->z = 0.2;
+        o9.b1->w = 0.1;
+
+        //o2.x = 2.3;
+        //o2.y = 1.2;
+        //o2.z = 0.2;
+        //o2.w = 0.1;
+
+
+        //o6.name = "Ball";
+        o9.b2->name="Ball";
+        //o6.size = {1.3, 2.1, 3.2};
+        o9.b2->size={1.3, 2.1, 3.2};
+        o9.b2->p1= {12.5, 16.4, 17.3};
+        //o6.p1 = {12.5, 16.4, 17.3};
+        // Using association relation quaternion
+        o9.b2->x= 1.1;
+        o9.b2->y = 1.4;
+        o9.b2->z = 1.6;
+        o9.b2->w = 1.9;
+
+        //o6.x = 1.1;
+        //o6.y = 1.4;
+        //o6.z = 1.6;
+        //o6.w = 1.9;
 
         //ros::Publisher turtle_vel
              //=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
@@ -130,11 +155,12 @@
                     continue;
 
                 }
-               // I did for one joint .If logically correct then will do for other joints .so far program is compiling
+
 
                o5a.p1.push_back(transformStamped.transform.translation.x);
                o5a.p1.push_back(transformStamped.transform.translation.y);
-               o5a.p1.push_back(transformStamped.transform.translation.y);
+               o5a.p1.push_back(transformStamped.transform.translation.z);
+
 
                o5a.or2.w=transformStamped.transform.rotation.w;
                o5a.or2.x=transformStamped.transform.rotation.x;
@@ -142,7 +168,6 @@
                o5a.or2.z=transformStamped.transform.rotation.z;
 
               }
-
             rate.sleep();
         }