Skip to content
Snippets Groups Projects
Commit 4f99c6b9 authored by Nikhil Ambardar's avatar Nikhil Ambardar
Browse files

Update robot_models_node.cpp

parent 1ab2e004
Branches
No related tags found
No related merge requests found
...@@ -78,59 +78,45 @@ ...@@ -78,59 +78,45 @@
o5g.p1 = {5.1, 18.3, 4.2}; o5g.p1 = {5.1, 18.3, 4.2};
o9.c->robname="Robot FPE"; o9.c->robname="Robot FPE";
//o8.robname = "Robot FPE";
//o8.of = true;
o9.c->of=true; o9.c->of=true;
o9.c->moving=false; o9.c->moving=false;
//o8.moving = false;
o9.c->grasppos=true; o9.c->grasppos=true;
//o8.grasppos = true;
//o8.planninggrp = "Panda Teaching";
o9.c->planninggrp= "Panda Teaching"; o9.c->planninggrp= "Panda Teaching";
//o3.grippos = {12, 14, 18};
o8.h->grippos= {12.1, 14.3, 18.2}; o8.h->grippos= {12.1, 14.3, 18.2};
o9.a->humannum = 1; o9.a->humannum = 1;
o9.b1->name="Cube"; o9.b1->name="Cube";
//o2.name = "Cube";
o9.b1->size={5.2, 7.5, 9.1}; 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}; 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->x= 2.3;
o9.b1->y = 1.2; o9.b1->y = 1.2;
o9.b1->z = 0.2; o9.b1->z = 0.2;
o9.b1->w = 0.1; 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"; o9.b2->name="Ball";
//o6.size = {1.3, 2.1, 3.2};
o9.b2->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}; 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->x= 1.1;
o9.b2->y = 1.4; o9.b2->y = 1.4;
o9.b2->z = 1.6; o9.b2->z = 1.6;
o9.b2->w = 1.9; 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);
tf2_ros::Buffer tfBuffer; tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
...@@ -163,71 +149,133 @@ ...@@ -163,71 +149,133 @@
} }
//o5a.p1[0] = 0.0; o5a.p1[0]=0.0;
o5a.p1[1]=0.0;
o5a.p1[2]=0.0;
o5b.p1[0]=0.0;
o5b.p1[1]=0.0;
o5b.p1[2]=0.0;
o5c.p1[0]=0.0;
o5c.p1[1]=0.0;
o5c.p1[2]=0.0;
o5d.p1[0]=0.0;
o5d.p1[1]=0.0;
o5d.p1[2]=0.0;
o5e.p1[0]=0.0;
o5e.p1[1]=0.0;
o5e.p1[2]=0.0;
o5f.p1[0]=0.0;
o5f.p1[1]=0.0;
o5f.p1[2]=0.0;
o5g.p1[0]=0.0;
o5g.p1[1]=0.0;
o5g.p1[2]=0.0;
o5a.p1[0]=transformStamped1.transform.translation.x; o5a.p1[0]=transformStamped1.transform.translation.x;
o5a.p1[1]=transformStamped1.transform.translation.y; o5a.p1[1]=transformStamped1.transform.translation.y;
o5a.p1[2]=transformStamped1.transform.translation.z; o5a.p1[2]=transformStamped1.transform.translation.z;
//ROS_INFO("%f",&o5a.p1); ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< o5a.p1[0] << ", y = "<< o5a.p1[1]<<", z = "<<o5a.p1[2]);
o5b.p1[0]=transformStamped2.transform.translation.x; o5b.p1[0]=transformStamped2.transform.translation.x;
o5b.p1[1]=transformStamped2.transform.translation.y; o5b.p1[1]=transformStamped2.transform.translation.y;
o5b.p1[2]=transformStamped2.transform.translation.z; o5b.p1[2]=transformStamped2.transform.translation.z;
ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o5b.p1[0] << ", y = "<< o5b.p1[1]<<", z = "<<o5b.p1[2]);
o5c.p1[0]=transformStamped3.transform.translation.x; o5c.p1[0]=transformStamped3.transform.translation.x;
o5c.p1[1]=transformStamped3.transform.translation.y; o5c.p1[1]=transformStamped3.transform.translation.y;
o5c.p1[2]=transformStamped3.transform.translation.z; o5c.p1[2]=transformStamped3.transform.translation.z;
ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o5c.p1[0] << ", y = "<< o5c.p1[1]<<", z = "<<o5c.p1[2]);
o5d.p1[0]=transformStamped4.transform.translation.x; o5d.p1[0]=transformStamped4.transform.translation.x;
o5d.p1[1]=transformStamped4.transform.translation.y; o5d.p1[1]=transformStamped4.transform.translation.y;
o5d.p1[2]=transformStamped4.transform.translation.z; o5d.p1[2]=transformStamped4.transform.translation.z;
ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o5d.p1[0] << ", y = "<< o5d.p1[1]<<", z = "<<o5d.p1[2]);
o5e.p1[0]=transformStamped5.transform.translation.x; o5e.p1[0]=transformStamped5.transform.translation.x;
o5e.p1[1]=transformStamped5.transform.translation.y; o5e.p1[1]=transformStamped5.transform.translation.y;
o5e.p1[2]=transformStamped5.transform.translation.z; o5e.p1[2]=transformStamped5.transform.translation.z;
ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o5e.p1[0] << ", y = "<< o5e.p1[1]<<", z = "<<o5e.p1[2]);
o5f.p1[0]=transformStamped6.transform.translation.x; o5f.p1[0]=transformStamped6.transform.translation.x;
o5f.p1[1]=transformStamped6.transform.translation.y; o5f.p1[1]=transformStamped6.transform.translation.y;
o5f.p1[2]=transformStamped6.transform.translation.z; o5f.p1[2]=transformStamped6.transform.translation.z;
ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o5f.p1[0] << ", y = "<< o5f.p1[1]<<", z = "<<o5f.p1[2]);
o5g.p1[0]=transformStamped7.transform.translation.x; o5g.p1[0]=transformStamped7.transform.translation.x;
o5g.p1[1]=transformStamped7.transform.translation.y; o5g.p1[1]=transformStamped7.transform.translation.y;
o5g.p1[2]=transformStamped7.transform.translation.z; o5g.p1[2]=transformStamped7.transform.translation.z;
ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o5g.p1[0] << ", y = "<< o5g.p1[1]<<", z = "<<o5g.p1[2]);
o5a.or2.w=0.0;
o5a.or2.x=0.0;
o5a.or2.y=0.0;
o5a.or2.z=0.0;
o5b.or2.w=0.0;
o5b.or2.x=0.0;
o5b.or2.y=0.0;
o5b.or2.z=0.0;
o5c.or2.w=0.0;
o5c.or2.x=0.0;
o5c.or2.y=0.0;
o5c.or2.z=0.0;
o5d.or2.w=0.0;
o5d.or2.x=0.0;
o5d.or2.y=0.0;
o5d.or2.z=0.0;
o5e.or2.w=0.0;
o5e.or2.x=0.0;
o5e.or2.y=0.0;
o5e.or2.z=0.0;
o5f.or2.w=0.0;
o5f.or2.x=0.0;
o5f.or2.y=0.0;
o5f.or2.z=0.0;
o5g.or2.w=0.0;
o5g.or2.x=0.0;
o5g.or2.y=0.0;
o5g.or2.z=0.0;
o5a.or2.w=transformStamped1.transform.rotation.w; o5a.or2.w=transformStamped1.transform.rotation.w;
o5a.or2.x=transformStamped1.transform.rotation.x; o5a.or2.x=transformStamped1.transform.rotation.x;
o5a.or2.y=transformStamped1.transform.rotation.y; o5a.or2.y=transformStamped1.transform.rotation.y;
o5a.or2.z=transformStamped1.transform.rotation.z; o5a.or2.z=transformStamped1.transform.rotation.z;
ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o5a.or2.w<<", x = "<<o5a.or2.x<<", y = "<<o5a.or2.y<<", z = "<<o5a.or2.z);
o5b.or2.w=transformStamped2.transform.rotation.w; o5b.or2.w=transformStamped2.transform.rotation.w;
o5b.or2.x=transformStamped2.transform.rotation.x; o5b.or2.x=transformStamped2.transform.rotation.x;
o5b.or2.y=transformStamped2.transform.rotation.y; o5b.or2.y=transformStamped2.transform.rotation.y;
o5b.or2.z=transformStamped2.transform.rotation.z; o5b.or2.z=transformStamped2.transform.rotation.z;
ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< o5b.or2.w<<", x = "<<o5b.or2.x<<", y = "<<o5b.or2.y<<", z = "<<o5b.or2.z);
o5c.or2.w=transformStamped3.transform.rotation.w; o5c.or2.w=transformStamped3.transform.rotation.w;
o5c.or2.x=transformStamped3.transform.rotation.x; o5c.or2.x=transformStamped3.transform.rotation.x;
o5c.or2.y=transformStamped3.transform.rotation.y; o5c.or2.y=transformStamped3.transform.rotation.y;
o5c.or2.z=transformStamped3.transform.rotation.z; o5c.or2.z=transformStamped3.transform.rotation.z;
ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o5c.or2.w<<", x = "<<o5c.or2.x<<", y = "<<o5c.or2.y<<", z = "<<o5c.or2.z);
o5d.or2.w=transformStamped4.transform.rotation.w; o5d.or2.w=transformStamped4.transform.rotation.w;
o5d.or2.x=transformStamped4.transform.rotation.x; o5d.or2.x=transformStamped4.transform.rotation.x;
o5d.or2.y=transformStamped4.transform.rotation.y; o5d.or2.y=transformStamped4.transform.rotation.y;
o5d.or2.z=transformStamped4.transform.rotation.z; o5d.or2.z=transformStamped4.transform.rotation.z;
ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<< o5d.or2.w<<", x = "<<o5d.or2.x<<", y = "<<o5d.or2.y<<", z = "<<o5d.or2.z);
o5e.or2.w=transformStamped5.transform.rotation.w; o5e.or2.w=transformStamped5.transform.rotation.w;
o5e.or2.x=transformStamped5.transform.rotation.x; o5e.or2.x=transformStamped5.transform.rotation.x;
o5e.or2.y=transformStamped5.transform.rotation.y; o5e.or2.y=transformStamped5.transform.rotation.y;
o5e.or2.z=transformStamped5.transform.rotation.z; o5e.or2.z=transformStamped5.transform.rotation.z;
ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o5e.or2.w<<", x = "<<o5e.or2.x<<", y = "<<o5e.or2.y<<", z = "<<o5e.or2.z);
o5f.or2.w=transformStamped6.transform.rotation.w; o5f.or2.w=transformStamped6.transform.rotation.w;
o5f.or2.x=transformStamped6.transform.rotation.x; o5f.or2.x=transformStamped6.transform.rotation.x;
o5f.or2.y=transformStamped6.transform.rotation.y; o5f.or2.y=transformStamped6.transform.rotation.y;
o5f.or2.z=transformStamped6.transform.rotation.z; o5f.or2.z=transformStamped6.transform.rotation.z;
ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o5f.or2.w<<", x = "<<o5f.or2.x<<", y = "<<o5f.or2.y<<", z = "<<o5f.or2.z);
o5g.or2.w=transformStamped7.transform.rotation.w; o5g.or2.w=transformStamped7.transform.rotation.w;
o5g.or2.x=transformStamped7.transform.rotation.x; o5g.or2.x=transformStamped7.transform.rotation.x;
o5g.or2.y=transformStamped7.transform.rotation.y; o5g.or2.y=transformStamped7.transform.rotation.y;
o5g.or2.z=transformStamped7.transform.rotation.z; o5g.or2.z=transformStamped7.transform.rotation.z;
ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<< o5g.or2.w<<", x = "<<o5g.or2.x<<", y = "<<o5g.or2.y<<", z = "<<o5g.or2.z);
} }
rate.sleep(); rate.sleep();
...@@ -235,4 +283,4 @@ ...@@ -235,4 +283,4 @@
return 0; return 0;
} }
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment