diff --git a/cmake-build-debug/CMakeFiles/Progress/12 b/cmake-build-debug/CMakeFiles/Progress/12 deleted file mode 100644 index 7b4d68d70fcae134d5348f5e118f5e9c9d3f05f6..0000000000000000000000000000000000000000 --- a/cmake-build-debug/CMakeFiles/Progress/12 +++ /dev/null @@ -1 +0,0 @@ -empty \ No newline at end of file diff --git a/cmake-build-debug/CMakeFiles/Progress/count.txt b/cmake-build-debug/CMakeFiles/Progress/count.txt deleted file mode 100644 index 0cfbf08886fca9a91cb753ec8734c84fcbe52c9f..0000000000000000000000000000000000000000 --- a/cmake-build-debug/CMakeFiles/Progress/count.txt +++ /dev/null @@ -1 +0,0 @@ -2 diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache b/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache index a35af57e673b44fa48495652d02fc391e7011147..f5b9d97cd241e558cc8a4b44e89b0427f89a8e6b 100644 --- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache +++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/CXX.includecache @@ -39,6 +39,8 @@ vector /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h worldobject.h /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h +joint.h +/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h vector - @@ -87,6 +89,8 @@ datalink/worldobject.h /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h datalink/quaternion.h /home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h +datalink/joint.h +/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h vector - iostream diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o b/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o new file mode 100644 index 0000000000000000000000000000000000000000..5a059d3a44812a8c935d6ebcf8daa66d7afc759a Binary files /dev/null and b/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o differ diff --git a/cmake-build-debug/devel/lib/robot_models/robot_models_node b/cmake-build-debug/devel/lib/robot_models/robot_models_node index d23265e6b24e7dfc254e7e7cb5220a18cdc71ea8..257958c9c035a2498ae5744f8534265e939d1c3d 100755 Binary files a/cmake-build-debug/devel/lib/robot_models/robot_models_node and b/cmake-build-debug/devel/lib/robot_models/robot_models_node differ diff --git a/src/datalink/robot.h b/src/datalink/robot.h index 84a18aee2d1d2a9ff01127d297afbd1a82dfb14b..92ed55e352d8300d0d0d7656f7a92c9fc95766c0 100644 --- a/src/datalink/robot.h +++ b/src/datalink/robot.h @@ -1,4 +1,5 @@ #include "worldobject.h" +#include "joint.h" #include <vector> #ifndef ROBOT_MODELS_R_H #define ROBOT_MODELS_R_H diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp index be41ced1eb599112bd376b30119707571ed25491..52de81fd27329438add1da9bba4d0b1204b33c6a 100644 --- a/src/robot_models_node.cpp +++ b/src/robot_models_node.cpp @@ -9,6 +9,7 @@ #include "datalink/world.h" #include "datalink/worldobject.h" #include "datalink/quaternion.h" +#include "datalink/joint.h" #include <vector> #include <iostream> #include <tf2_ros/transform_listener.h> @@ -44,6 +45,8 @@ void pub(const std_msgs::UInt32::ConstPtr& msg) spinner.start(); ROS_INFO("HELLO, WORLD"); + world world1; + auto robot1 = new robotfpe; abstractobject o1; graspobject o2; hand o3; @@ -56,35 +59,35 @@ void pub(const std_msgs::UInt32::ConstPtr& msg) joint o5f; joint o5g; obstacle o6; - robot robot1; + //robot robot1; robotfpe o8; world o9; worldobject o10; - robot1.jo.push_back(o5a); - robot1.jo.push_back(o5b); - robot1.jo.push_back(o5c); - robot1.jo.push_back(o5d); - robot1.jo.push_back(o5e); - robot1.jo.push_back(o5f); - robot1.jo.push_back(o5g); + robot1->jo.push_back(o5a); + robot1->jo.push_back(o5b); + robot1->jo.push_back(o5c); + robot1->jo.push_back(o5d); + robot1->jo.push_back(o5e); + robot1->jo.push_back(o5f); + robot1->jo.push_back(o5g); int yx = 0; - robot1.jo[++yx].name = "panda_link1"; - robot1.jo[++yx].name = "panda_link2"; - robot1.jo[++yx].name = "panda_link3"; - robot1.jo[++yx].name = "panda_link4"; - robot1.jo[++yx].name = "panda_link5"; - robot1.jo[++yx].name = "panda_link6"; - robot1.jo[++yx].name = "panda_link7"; + robot1->jo[++yx].name = "panda_link1"; + robot1->jo[++yx].name = "panda_link2"; + robot1->jo[++yx].name = "panda_link3"; + robot1->jo[++yx].name = "panda_link4"; + robot1->jo[++yx].name = "panda_link5"; + robot1->jo[++yx].name = "panda_link6"; + robot1->jo[++yx].name = "panda_link7"; - o9.c->robname = "Robot FPE"; - o9.c->of = true; - o9.c->moving = false; - o9.c->grasppos = true; - o9.c->planninggrp = "Panda Teaching"; + robot1->robname = "Robot FPE"; + robot1->of = true; + robot1->moving = false; + robot1->grasppos = true; + robot1->planninggrp = "Panda Teaching"; - o8.h->grippos = {12.1, 14.3, 18.2}; + robot1->h->grippos = {12.1, 14.3, 18.2}; auto *obstacle1 = new obstacle{}; obstacle1->name = "Cube"; @@ -120,7 +123,7 @@ void pub(const std_msgs::UInt32::ConstPtr& msg) o9.b2->w = 1.9; */ ros::Subscriber sub = node_handle.subscribe("updateDectectedHumans", 1000, pub); - ros::spin(); + //ros::spin(); tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); @@ -138,13 +141,13 @@ void pub(const std_msgs::UInt32::ConstPtr& msg) geometry_msgs::TransformStamped transformStamped7; try { - transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0)); - transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0)); - transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0)); - transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0)); - transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0)); - transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0)); - transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0)); + transformStamped1 = tfBuffer.lookupTransform("world", robot1->jo[1].name, ros::Time(0)); + transformStamped2 = tfBuffer.lookupTransform("world", robot1->jo[2].name, ros::Time(0)); + transformStamped3 = tfBuffer.lookupTransform("world", robot1->jo[3].name, ros::Time(0)); + transformStamped4 = tfBuffer.lookupTransform("world", robot1->jo[4].name, ros::Time(0)); + transformStamped5 = tfBuffer.lookupTransform("world", robot1->jo[5].name, ros::Time(0)); + transformStamped6 = tfBuffer.lookupTransform("world", robot1->jo[6].name, ros::Time(0)); + transformStamped7 = tfBuffer.lookupTransform("world", robot1->jo[7].name, ros::Time(0)); o9.humans->humannum =gvar; } catch (tf2::TransformException &ex) { ROS_WARN("%s", ex.what()); @@ -152,155 +155,161 @@ void pub(const std_msgs::UInt32::ConstPtr& msg) continue; } - robot1.jo[3].p1[0] = 0.0; - robot1.jo[3].p1[1] = 0.0; - robot1.jo[3].p1[2] = 0.0; - robot1.jo[4].p1[0] = 0.0; - robot1.jo[4].p1[1] = 0.0; - robot1.jo[4].p1[2] = 0.0; - robot1.jo[5].p1[0] = 0.0; - robot1.jo[5].p1[1] = 0.0; - robot1.jo[5].p1[2] = 0.0; - robot1.jo[6].p1[0] = 0.0; - robot1.jo[6].p1[1] = 0.0; - robot1.jo[6].p1[2] = 0.0; - robot1.jo[7].p1[0] = 0.0; - robot1.jo[7].p1[1] = 0.0; - robot1.jo[7].p1[2] = 0.0; + robot1->jo[1].p1[0] = 0.0; + robot1->jo[1].p1[1] = 0.0; + robot1->jo[1].p1[2] = 0.0; + robot1->jo[2].p1[0] = 0.0; + robot1->jo[2].p1[1] = 0.0; + robot1->jo[2].p1[2] = 0.0; + robot1->jo[3].p1[0] = 0.0; + robot1->jo[3].p1[1] = 0.0; + robot1->jo[3].p1[2] = 0.0; + robot1->jo[4].p1[0] = 0.0; + robot1->jo[4].p1[1] = 0.0; + robot1->jo[4].p1[2] = 0.0; + robot1->jo[5].p1[0] = 0.0; + robot1->jo[5].p1[1] = 0.0; + robot1->jo[5].p1[2] = 0.0; + robot1->jo[6].p1[0] = 0.0; + robot1->jo[6].p1[1] = 0.0; + robot1->jo[6].p1[2] = 0.0; + robot1->jo[7].p1[0] = 0.0; + robot1->jo[7].p1[1] = 0.0; + robot1->jo[7].p1[2] = 0.0; - robot1.jo[1].p1[0] = transformStamped1.transform.translation.x; - robot1.jo[1].p1[1] = transformStamped1.transform.translation.y; - robot1.jo[1].p1[2] = transformStamped1.transform.translation.z; + robot1->jo[1].p1[0] = transformStamped1.transform.translation.x; + robot1->jo[1].p1[1] = transformStamped1.transform.translation.y; + robot1->jo[1].p1[2] = transformStamped1.transform.translation.z; ROS_INFO_STREAM( - "panda_link1 Position is " << " x = " << robot1.jo[1].p1[0] << ", y = " << robot1.jo[1].p1[1] - << ", z = " << robot1.jo[1].p1[2]); + "panda_link1 Position is " << " x = " << robot1->jo[1].p1[0] << ", y = " << robot1->jo[1].p1[1] + << ", z = " << robot1->jo[1].p1[2]); - robot1.jo[2].p1[0] = transformStamped2.transform.translation.x; - robot1.jo[2].p1[1] = transformStamped2.transform.translation.y; - robot1.jo[2].p1[2] = transformStamped2.transform.translation.z; + robot1->jo[2].p1[0] = transformStamped2.transform.translation.x; + robot1->jo[2].p1[1] = transformStamped2.transform.translation.y; + robot1->jo[2].p1[2] = transformStamped2.transform.translation.z; ROS_INFO_STREAM( - "panda_link2 Position is " << " x = " << robot1.jo[2].p1[0] << ", y = " << robot1.jo[2].p1[1] - << ", z = " << robot1.jo[2].p1[2]); + "panda_link2 Position is " << " x = " << robot1->jo[2].p1[0] << ", y = " << robot1->jo[2].p1[1] + << ", z = " << robot1->jo[2].p1[2]); - robot1.jo[3].p1[0] = transformStamped3.transform.translation.x; - robot1.jo[3].p1[1] = transformStamped3.transform.translation.y; - robot1.jo[3].p1[2] = transformStamped3.transform.translation.z; + robot1->jo[3].p1[0] = transformStamped3.transform.translation.x; + robot1->jo[3].p1[1] = transformStamped3.transform.translation.y; + robot1->jo[3].p1[2] = transformStamped3.transform.translation.z; ROS_INFO_STREAM( - "panda_link3 Position is " << " x = " << robot1.jo[3].p1[0] << ", y = " << robot1.jo[3].p1[1] - << ", z = " << robot1.jo[3].p1[2]); + "panda_link3 Position is " << " x = " << robot1->jo[3].p1[0] << ", y = " << robot1->jo[3].p1[1] + << ", z = " << robot1->jo[3].p1[2]); - robot1.jo[4].p1[0] = transformStamped4.transform.translation.x; - robot1.jo[4].p1[1] = transformStamped4.transform.translation.y; - robot1.jo[4].p1[2] = transformStamped4.transform.translation.z; + robot1->jo[4].p1[0] = transformStamped4.transform.translation.x; + robot1->jo[4].p1[1] = transformStamped4.transform.translation.y; + robot1->jo[4].p1[2] = transformStamped4.transform.translation.z; ROS_INFO_STREAM( - "panda_link4 Position is " << " x = " << robot1.jo[4].p1[0] << ", y = " << robot1.jo[4].p1[1] - << ", z = " << robot1.jo[4].p1[2]); + "panda_link4 Position is " << " x = " << robot1->jo[4].p1[0] << ", y = " << robot1->jo[4].p1[1] + << ", z = " << robot1->jo[4].p1[2]); - robot1.jo[5].p1[0] = transformStamped5.transform.translation.x; - robot1.jo[5].p1[1] = transformStamped5.transform.translation.y; - robot1.jo[5].p1[2] = transformStamped5.transform.translation.z; + robot1->jo[5].p1[0] = transformStamped5.transform.translation.x; + robot1->jo[5].p1[1] = transformStamped5.transform.translation.y; + robot1->jo[5].p1[2] = transformStamped5.transform.translation.z; ROS_INFO_STREAM( - "panda_link5 Position is " << " x = " << robot1.jo[5].p1[0] << ", y = " << robot1.jo[5].p1[1] - << ", z = " << robot1.jo[5].p1[2]); + "panda_link5 Position is " << " x = " << robot1->jo[5].p1[0] << ", y = " << robot1->jo[5].p1[1] + << ", z = " << robot1->jo[5].p1[2]); - robot1.jo[6].p1[0] = transformStamped6.transform.translation.x; - robot1.jo[6].p1[1] = transformStamped6.transform.translation.y; - robot1.jo[6].p1[2] = transformStamped6.transform.translation.z; + robot1->jo[6].p1[0] = transformStamped6.transform.translation.x; + robot1->jo[6].p1[1] = transformStamped6.transform.translation.y; + robot1->jo[6].p1[2] = transformStamped6.transform.translation.z; ROS_INFO_STREAM( - "panda_link6 Position is " << " x = " << robot1.jo[6].p1[0] << ", y = " << robot1.jo[6].p1[1] - << ", z = " << robot1.jo[6].p1[2]); + "panda_link6 Position is " << " x = " << robot1->jo[6].p1[0] << ", y = " << robot1->jo[6].p1[1] + << ", z = " << robot1->jo[6].p1[2]); - robot1.jo[7].p1[0] = transformStamped7.transform.translation.x; - robot1.jo[7].p1[1] = transformStamped7.transform.translation.y; - robot1.jo[7].p1[2] = transformStamped7.transform.translation.z; + robot1->jo[7].p1[0] = transformStamped7.transform.translation.x; + robot1->jo[7].p1[1] = transformStamped7.transform.translation.y; + robot1->jo[7].p1[2] = transformStamped7.transform.translation.z; ROS_INFO_STREAM( - "panda_link7 Position is " << " x = " << robot1.jo[7].p1[0] << ", y = " << robot1.jo[7].p1[1] - << ", z = " << robot1.jo[7].p1[2]); + "panda_link7 Position is " << " x = " << robot1->jo[7].p1[0] << ", y = " << robot1->jo[7].p1[1] + << ", z = " << robot1->jo[7].p1[2]); - robot1.jo[1].or2.w = 0.0; - robot1.jo[1].or2.x = 0.0; - robot1.jo[1].or2.y = 0.0; - robot1.jo[1].or2.z = 0.0; - robot1.jo[2].or2.w = 0.0; - robot1.jo[2].or2.x = 0.0; - robot1.jo[2].or2.y = 0.0; - robot1.jo[2].or2.z = 0.0; - robot1.jo[3].or2.w = 0.0; - robot1.jo[3].or2.x = 0.0; - robot1.jo[3].or2.y = 0.0; - robot1.jo[3].or2.z = 0.0; - robot1.jo[4].or2.w = 0.0; - robot1.jo[4].or2.x = 0.0; - robot1.jo[4].or2.y = 0.0; - robot1.jo[4].or2.z = 0.0; - robot1.jo[5].or2.w = 0.0; - robot1.jo[5].or2.x = 0.0; - robot1.jo[5].or2.y = 0.0; - robot1.jo[5].or2.z = 0.0; - robot1.jo[6].or2.w = 0.0; - robot1.jo[6].or2.x = 0.0; - robot1.jo[6].or2.y = 0.0; - robot1.jo[6].or2.z = 0.0; - robot1.jo[7].or2.w = 0.0; - robot1.jo[7].or2.x = 0.0; - robot1.jo[7].or2.y = 0.0; - robot1.jo[7].or2.z = 0.0; + robot1->jo[1].or2.w = 0.0; + robot1->jo[1].or2.x = 0.0; + robot1->jo[1].or2.y = 0.0; + robot1->jo[1].or2.z = 0.0; + robot1->jo[2].or2.w = 0.0; + robot1->jo[2].or2.x = 0.0; + robot1->jo[2].or2.y = 0.0; + robot1->jo[2].or2.z = 0.0; + robot1->jo[3].or2.w = 0.0; + robot1->jo[3].or2.x = 0.0; + robot1->jo[3].or2.y = 0.0; + robot1->jo[3].or2.z = 0.0; + robot1->jo[4].or2.w = 0.0; + robot1->jo[4].or2.x = 0.0; + robot1->jo[4].or2.y = 0.0; + robot1->jo[4].or2.z = 0.0; + robot1->jo[5].or2.w = 0.0; + robot1->jo[5].or2.x = 0.0; + robot1->jo[5].or2.y = 0.0; + robot1->jo[5].or2.z = 0.0; + robot1->jo[6].or2.w = 0.0; + robot1->jo[6].or2.x = 0.0; + robot1->jo[6].or2.y = 0.0; + robot1->jo[6].or2.z = 0.0; + robot1->jo[7].or2.w = 0.0; + robot1->jo[7].or2.x = 0.0; + robot1->jo[7].or2.y = 0.0; + robot1->jo[7].or2.z = 0.0; - robot1.jo[1].or2.w = transformStamped1.transform.rotation.w; - robot1.jo[1].or2.x = transformStamped1.transform.rotation.x; - robot1.jo[1].or2.y = transformStamped1.transform.rotation.y; - robot1.jo[1].or2.z = transformStamped1.transform.rotation.z; + robot1->jo[1].or2.w = transformStamped1.transform.rotation.w; + robot1->jo[1].or2.x = transformStamped1.transform.rotation.x; + robot1->jo[1].or2.y = transformStamped1.transform.rotation.y; + robot1->jo[1].or2.z = transformStamped1.transform.rotation.z; ROS_INFO_STREAM( - "panda_link1 Orientation is " << " w = " << robot1.jo[1].or2.w << ", x = " << robot1.jo[1].or2.x - << ", y = " << robot1.jo[1].or2.y << ", z = " << robot1.jo[1].or2.z); + "panda_link1 Orientation is " << " w = " << robot1->jo[1].or2.w << ", x = " << robot1->jo[1].or2.x + << ", y = " << robot1->jo[1].or2.y << ", z = " << robot1->jo[1].or2.z); - robot1.jo[2].or2.w = transformStamped2.transform.rotation.w; - robot1.jo[2].or2.x = transformStamped2.transform.rotation.x; - robot1.jo[2].or2.y = transformStamped2.transform.rotation.y; - robot1.jo[2].or2.z = transformStamped2.transform.rotation.z; + robot1->jo[2].or2.w = transformStamped2.transform.rotation.w; + robot1->jo[2].or2.x = transformStamped2.transform.rotation.x; + robot1->jo[2].or2.y = transformStamped2.transform.rotation.y; + robot1->jo[2].or2.z = transformStamped2.transform.rotation.z; ROS_INFO_STREAM( - "panda_link2 Orientation is " << " w = " << robot1.jo[2].or2.w << ", x = " << robot1.jo[2].or2.x - << ", y = " << robot1.jo[2].or2.y << ", z = " << robot1.jo[2].or2.z); + "panda_link2 Orientation is " << " w = " << robot1->jo[2].or2.w << ", x = " << robot1->jo[2].or2.x + << ", y = " << robot1->jo[2].or2.y << ", z = " << robot1->jo[2].or2.z); - robot1.jo[3].or2.w = transformStamped3.transform.rotation.w; - robot1.jo[3].or2.x = transformStamped3.transform.rotation.x; - robot1.jo[3].or2.y = transformStamped3.transform.rotation.y; - robot1.jo[3].or2.z = transformStamped3.transform.rotation.z; + robot1->jo[3].or2.w = transformStamped3.transform.rotation.w; + robot1->jo[3].or2.x = transformStamped3.transform.rotation.x; + robot1->jo[3].or2.y = transformStamped3.transform.rotation.y; + robot1->jo[3].or2.z = transformStamped3.transform.rotation.z; ROS_INFO_STREAM( - "panda_link3 Orientation is " << " w = " << robot1.jo[3].or2.w << ", x = " << robot1.jo[3].or2.x - << ", y = " << robot1.jo[3].or2.y << ", z = " << robot1.jo[3].or2.z); + "panda_link3 Orientation is " << " w = " << robot1->jo[3].or2.w << ", x = " << robot1->jo[3].or2.x + << ", y = " << robot1->jo[3].or2.y << ", z = " << robot1->jo[3].or2.z); - robot1.jo[4].or2.w = transformStamped4.transform.rotation.w; - robot1.jo[4].or2.x = transformStamped4.transform.rotation.x; - robot1.jo[4].or2.y = transformStamped4.transform.rotation.y; - robot1.jo[4].or2.z = transformStamped4.transform.rotation.z; + robot1->jo[4].or2.w = transformStamped4.transform.rotation.w; + robot1->jo[4].or2.x = transformStamped4.transform.rotation.x; + robot1->jo[4].or2.y = transformStamped4.transform.rotation.y; + robot1->jo[4].or2.z = transformStamped4.transform.rotation.z; ROS_INFO_STREAM( - "panda_link4 Orientation is " << " w = " << robot1.jo[4].or2.w << ", x = " << robot1.jo[4].or2.x - << ", y = " << robot1.jo[4].or2.y << ", z = " << robot1.jo[4].or2.z); + "panda_link4 Orientation is " << " w = " << robot1->jo[4].or2.w << ", x = " << robot1->jo[4].or2.x + << ", y = " << robot1->jo[4].or2.y << ", z = " << robot1->jo[4].or2.z); - robot1.jo[5].or2.w = transformStamped5.transform.rotation.w; - robot1.jo[5].or2.x = transformStamped5.transform.rotation.x; - robot1.jo[5].or2.y = transformStamped5.transform.rotation.y; - robot1.jo[5].or2.z = transformStamped5.transform.rotation.z; + robot1->jo[5].or2.w = transformStamped5.transform.rotation.w; + robot1->jo[5].or2.x = transformStamped5.transform.rotation.x; + robot1->jo[5].or2.y = transformStamped5.transform.rotation.y; + robot1->jo[5].or2.z = transformStamped5.transform.rotation.z; ROS_INFO_STREAM( - "panda_link5 Orientation is " << " w = " << robot1.jo[5].or2.w << ", x = " << robot1.jo[5].or2.x - << ", y = " << robot1.jo[5].or2.y << ", z = " << robot1.jo[5].or2.z); + "panda_link5 Orientation is " << " w = " << robot1->jo[5].or2.w << ", x = " << robot1->jo[5].or2.x + << ", y = " << robot1->jo[5].or2.y << ", z = " << robot1->jo[5].or2.z); - robot1.jo[6].or2.w = transformStamped6.transform.rotation.w; - robot1.jo[6].or2.x = transformStamped6.transform.rotation.x; - robot1.jo[6].or2.y = transformStamped6.transform.rotation.y; - robot1.jo[6].or2.z = transformStamped6.transform.rotation.z; + robot1->jo[6].or2.w = transformStamped6.transform.rotation.w; + robot1->jo[6].or2.x = transformStamped6.transform.rotation.x; + robot1->jo[6].or2.y = transformStamped6.transform.rotation.y; + robot1->jo[6].or2.z = transformStamped6.transform.rotation.z; ROS_INFO_STREAM( - "panda_link6 Orientation is " << " w = " << robot1.jo[6].or2.w << ", x = " << robot1.jo[6].or2.x - << ", y = " << robot1.jo[6].or2.y << ", z = " << robot1.jo[6].or2.z); + "panda_link6 Orientation is " << " w = " << robot1->jo[6].or2.w << ", x = " << robot1->jo[6].or2.x + << ", y = " << robot1->jo[6].or2.y << ", z = " << robot1->jo[6].or2.z); - robot1.jo[7].or2.w = transformStamped7.transform.rotation.w; - robot1.jo[7].or2.x = transformStamped7.transform.rotation.x; - robot1.jo[7].or2.y = transformStamped7.transform.rotation.y; - robot1.jo[7].or2.z = transformStamped7.transform.rotation.z; + robot1->jo[7].or2.w = transformStamped7.transform.rotation.w; + robot1->jo[7].or2.x = transformStamped7.transform.rotation.x; + robot1->jo[7].or2.y = transformStamped7.transform.rotation.y; + robot1->jo[7].or2.z = transformStamped7.transform.rotation.z; ROS_INFO_STREAM( - "panda_link7 Orientation is " << " w = " << robot1.jo[7].or2.w << ", x = " << robot1.jo[7].or2.x - << ", y = " << robot1.jo[7].or2.y << ", z = " << robot1.jo[7].or2.z); + "panda_link7 Orientation is " << " w = " << robot1->jo[7].or2.w << ", x = " << robot1->jo[7].or2.x + << ", y = " << robot1->jo[7].or2.y << ", z = " << robot1->jo[7].or2.z); } rate.sleep(); }