diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp index 9b4d1bcb8edc732ed8d81a6827c2e839140f1767..c5a90f0d9675b6508735ac430271be483c80d94e 100644 --- a/src/robot_models_node.cpp +++ b/src/robot_models_node.cpp @@ -54,28 +54,60 @@ world o9; worldobject o10; - - 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}; + o7.jo1->name= "panda_link1"; + o7.jo2->name= "panda_link2"; + o7.jo3->name= "panda_link3"; + o7.jo4->name= "panda_link4"; + o7.jo5->name= "panda_link5"; + o7.jo6->name= "panda_link6"; + o7.jo7->name= "panda_link7"; + + //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"; + o7.jo1->or2= {1.2,5.3,6.5,7.4}; + o7.jo2->or2= {2.5,3.6,5.8,4.9}; + o7.jo3->or2= {5.1,1.2,9.9,6.8}; + o7.jo4->or2= {6.3,3.4,8.5,9.6}; + o7.jo5->or2= {8.2,2.6,7.9,5.5}; + o7.jo6->or2= {7.3,4.4,6.7,4.0}; + o7.jo7->or2= {7.3,4.4,6.7,4.0}; + + + //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}; + + o7.jo1->p1= {1.3, 10.1, 9.4}; + o7.jo2->p1= {14.7, 11.3, 6.5}; + o7.jo3->p1= {4.9, 13.8, 7.7}; + o7.jo4->p1= {2.4, 12.5, 1.6}; + o7.jo5->p1= {3.1, 16.2, 5.3}; + o7.jo6->p1= {11.5, 15.4, 3.4}; + o7.jo7->p1= {5.1, 18.3, 4.2}; + //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"; @@ -94,7 +126,7 @@ o9.b1->name="Cube"; o9.b1->size={5.2, 7.5, 9.1}; - ; + o9.b1->p1= {13.2, 14.1, 17.5}; o9.b1->x= 2.3; @@ -135,13 +167,13 @@ geometry_msgs::TransformStamped transformStamped7; try { - transformStamped1 = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0)); - transformStamped2 = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0)); - transformStamped3 = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0)); - transformStamped4 = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0)); - transformStamped5 = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0)); - transformStamped6 = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0)); - transformStamped7 = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0)); + transformStamped1 = tfBuffer.lookupTransform("world", o7.jo1->name, ros::Time(0)); + transformStamped2 = tfBuffer.lookupTransform("world", o7.jo2->name, ros::Time(0)); + transformStamped3 = tfBuffer.lookupTransform("world", o7.jo3->name, ros::Time(0)); + transformStamped4 = tfBuffer.lookupTransform("world", o7.jo4->name, ros::Time(0)); + transformStamped5 = tfBuffer.lookupTransform("world", o7.jo5->name, ros::Time(0)); + transformStamped6 = tfBuffer.lookupTransform("world", o7.jo6->name, ros::Time(0)); + transformStamped7 = tfBuffer.lookupTransform("world", o7.jo7->name, ros::Time(0)); } catch (tf2::TransformException &ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); @@ -149,64 +181,113 @@ } - 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[1]=transformStamped1.transform.translation.y; - o5a.p1[2]=transformStamped1.transform.translation.z; - 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[1]=transformStamped2.transform.translation.y; - 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[1]=transformStamped3.transform.translation.y; - 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[1]=transformStamped4.transform.translation.y; - 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[1]=transformStamped5.transform.translation.y; - 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[1]=transformStamped6.transform.translation.y; - 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[1]=transformStamped7.transform.translation.y; - 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; + o7.jo1->p1[0]=0.0; + o7.jo1->p1[1]=0.0; + o7.jo1->p1[2]=0.0; + o7.jo2->p1[0]=0.0; + o7.jo2->p1[1]=0.0; + o7.jo2->p1[2]=0.0; + o7.jo3->p1[0]=0.0; + o7.jo3->p1[1]=0.0; + o7.jo3->p1[2]=0.0; + o7.jo4->p1[0]=0.0; + o7.jo4->p1[1]=0.0; + o7.jo4->p1[2]=0.0; + o7.jo5->p1[0]=0.0; + o7.jo5->p1[1]=0.0; + o7.jo5->p1[2]=0.0; + o7.jo6->p1[0]=0.0; + o7.jo6->p1[1]=0.0; + o7.jo6->p1[2]=0.0; + o7.jo7->p1[0]=0.0; + o7.jo7->p1[1]=0.0; + o7.jo7->p1[2]=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; + + o7.jo1->p1[0]=transformStamped1.transform.translation.x; + o7.jo1->p1[1]=transformStamped1.transform.translation.y; + o7.jo1->p1[2]=transformStamped1.transform.translation.z; + ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< o7.jo1->p1[0] << ", y = "<< o7.jo1->p1[1]<<", z = "<<o7.jo1->p1[2]); + + o7.jo2->p1[0]=transformStamped2.transform.translation.x; + o7.jo2->p1[1]=transformStamped2.transform.translation.y; + o7.jo2->p1[2]=transformStamped2.transform.translation.z; + ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o7.jo2->p1[0] << ", y = "<< o7.jo2->p1[1]<<", z = "<<o7.jo2->p1[2]); + + o7.jo3->p1[0]=transformStamped3.transform.translation.x; + o7.jo3->p1[1]=transformStamped3.transform.translation.y; + o7.jo3->p1[2]=transformStamped3.transform.translation.z; + ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o7.jo3->p1[0] << ", y = "<< o7.jo3->p1[1]<<", z = "<<o7.jo3->p1[2]); + + o7.jo4->p1[0]=transformStamped4.transform.translation.x; + o7.jo4->p1[1]=transformStamped4.transform.translation.y; + o7.jo4->p1[2]=transformStamped4.transform.translation.z; + ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o7.jo4->p1[0] << ", y = "<< o7.jo4->p1[1]<<", z = "<<o7.jo4->p1[2]); + + o7.jo5->p1[0]=transformStamped5.transform.translation.x; + o7.jo5->p1[1]=transformStamped5.transform.translation.y; + o7.jo5->p1[2]=transformStamped5.transform.translation.z; + ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o7.jo5->p1[0] << ", y = "<< o7.jo5->p1[1]<<", z = "<<o7.jo5->p1[2]); + + o7.jo6->p1[0]=transformStamped6.transform.translation.x; + o7.jo6->p1[1]=transformStamped6.transform.translation.y; + o7.jo6->p1[2]=transformStamped6.transform.translation.z; + ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo6->p1[0] << ", y = "<< o7.jo6->p1[1]<<", z = "<<o7.jo6->p1[2]); + + o7.jo7->p1[0]=transformStamped7.transform.translation.x; + o7.jo7->p1[0]=transformStamped7.transform.translation.y; + o7.jo7->p1[0]=transformStamped7.transform.translation.z; + ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo7->p1[0] << ", y = "<< o7.jo7->p1[1]<<", z = "<<o7.jo7->p1[2]); + + o7.jo1->or2.w=0.0; + o7.jo1->or2.x=0.0; + o7.jo1->or2.y=0.0; + o7.jo1->or2.z=0.0; + o7.jo2->or2.w=0.0; + o7.jo2->or2.x=0.0; + o7.jo2->or2.y=0.0; + o7.jo2->or2.z=0.0; + o7.jo3->or2.w=0.0; + o7.jo3->or2.x=0.0; + o7.jo3->or2.y=0.0; + o7.jo3->or2.z=0.0; + o7.jo4->or2.w=0.0; + o7.jo4->or2.x=0.0; + o7.jo4->or2.y=0.0; + o7.jo4->or2.z=0.0; + o7.jo5->or2.w=0.0; + o7.jo5->or2.x=0.0; + o7.jo5->or2.y=0.0; + o7.jo5->or2.z=0.0; + o7.jo6->or2.w=0.0; + o7.jo6->or2.x=0.0; + o7.jo6->or2.y=0.0; + o7.jo6->or2.z=0.0; + o7.jo7->or2.w=0.0; + o7.jo7->or2.x=0.0; + o7.jo7->or2.y=0.0; + o7.jo7->or2.z=0.0; + /*o5a.or2.w=0.0; o5a.or2.x=0.0; o5a.or2.y=0.0; o5a.or2.z=0.0; @@ -233,49 +314,49 @@ 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.x=transformStamped1.transform.rotation.x; - o5a.or2.y=transformStamped1.transform.rotation.y; - 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.x=transformStamped2.transform.rotation.x; - o5b.or2.y=transformStamped2.transform.rotation.y; - 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.x=transformStamped3.transform.rotation.x; - o5c.or2.y=transformStamped3.transform.rotation.y; - 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.x=transformStamped4.transform.rotation.x; - o5d.or2.y=transformStamped4.transform.rotation.y; - 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.x=transformStamped5.transform.rotation.x; - o5e.or2.y=transformStamped5.transform.rotation.y; - 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.x=transformStamped6.transform.rotation.x; - o5f.or2.y=transformStamped6.transform.rotation.y; - 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.x=transformStamped7.transform.rotation.x; - o5g.or2.y=transformStamped7.transform.rotation.y; - 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); + o5g.or2.z=0.0;*/ + + o7.jo1->or2.w=transformStamped1.transform.rotation.w; + o7.jo1->or2.x=transformStamped1.transform.rotation.x; + o7.jo1->or2.y=transformStamped1.transform.rotation.y; + o7.jo1->or2.z=transformStamped1.transform.rotation.z; + ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o7.jo1->or2.w<<", x = "<<o7.jo1->or2.x<<", y = "<<o7.jo1->or2.y<<", z = "<<o7.jo1->or2.z); + + o7.jo2->or2.w=transformStamped2.transform.rotation.w; + o7.jo2->or2.x=transformStamped2.transform.rotation.x; + o7.jo2->or2.y=transformStamped2.transform.rotation.y; + o7.jo2->or2.z=transformStamped2.transform.rotation.z; + ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< o7.jo2->or2.w<<", x = "<< o7.jo2->or2.x<<", y = "<< o7.jo2->or2.y<<", z = "<< o7.jo2->or2.z); + + o7.jo3->or2.w=transformStamped3.transform.rotation.w; + o7.jo3->or2.x=transformStamped3.transform.rotation.x; + o7.jo3->or2.y=transformStamped3.transform.rotation.y; + o7.jo3->or2.z=transformStamped3.transform.rotation.z; + ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o7.jo3->or2.w<<", x = "<<o7.jo3->or2.x<<", y = "<<o7.jo3->or2.y<<", z = "<<o7.jo3->or2.z); + + o7.jo4->or2.w=transformStamped4.transform.rotation.w; + o7.jo4->or2.x=transformStamped4.transform.rotation.x; + o7.jo4->or2.y=transformStamped4.transform.rotation.y; + o7.jo4->or2.z=transformStamped4.transform.rotation.z; + ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<o7.jo4->or2.w<<", x = "<<o7.jo4->or2.x<<", y = "<<o7.jo4->or2.y<<", z = "<<o7.jo4->or2.z); + + o7.jo5->or2.w=transformStamped5.transform.rotation.w; + o7.jo5->or2.x=transformStamped5.transform.rotation.x; + o7.jo5->or2.y=transformStamped5.transform.rotation.y; + o7.jo5->or2.z=transformStamped5.transform.rotation.z; + ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o7.jo5->or2.w<<", x = "<<o7.jo5->or2.x<<", y = "<<o7.jo5->or2.y<<", z = "<<o7.jo5->or2.z); + + o7.jo6->or2.w=transformStamped6.transform.rotation.w; + o7.jo6->or2.x=transformStamped6.transform.rotation.x; + o7.jo6->or2.y=transformStamped6.transform.rotation.y; + o7.jo6->or2.z=transformStamped6.transform.rotation.z; + ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o7.jo6->or2.w<<", x = "<<o7.jo6->or2.x<<", y = "<<o7.jo6->or2.y<<", z = "<<o7.jo6->or2.z); + + o7.jo7->or2.w=transformStamped7.transform.rotation.w; + o7.jo7->or2.x=transformStamped7.transform.rotation.x; + o7.jo7->or2.y=transformStamped7.transform.rotation.y; + o7.jo7->or2.z=transformStamped7.transform.rotation.z; + ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<o7.jo7->or2.w<<", x = "<<o7.jo7->or2.x<<", y = "<<o7.jo7->or2.y<<", z = "<<o7.jo7->or2.z); } rate.sleep();