diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp index 6e3484c7a58bad63c7e7f08384e6833685e56ea8..b37fe89995b41138f276d90a6dad1b0157cfd442 100644 --- a/src/robot_models_node.cpp +++ b/src/robot_models_node.cpp @@ -157,20 +157,75 @@ } - o5a.p1.push_back(transformStamped.transform.translation.x); - o5a.p1.push_back(transformStamped.transform.translation.y); - o5a.p1.push_back(transformStamped.transform.translation.z); - + o5a.p1.push_back(transformStamped.transform.translation.x); + o5a.p1.push_back(transformStamped.transform.translation.y); + o5a.p1.push_back(transformStamped.transform.translation.z); + o5a.p1[0] = 0.0; + o5b.p1.push_back(transformStamped.transform.translation.x); + o5b.p1.push_back(transformStamped.transform.translation.y); + o5b.p1.push_back(transformStamped.transform.translation.z); + o5b.p1[0] = 0.0; + o5c.p1.push_back(transformStamped.transform.translation.x); + o5c.p1.push_back(transformStamped.transform.translation.y); + o5c.p1.push_back(transformStamped.transform.translation.z); + o5c.p1[0] = 0.0; + o5d.p1.push_back(transformStamped.transform.translation.x); + o5d.p1.push_back(transformStamped.transform.translation.y); + o5d.p1.push_back(transformStamped.transform.translation.z); + o5d.p1[0] = 0.0; + o5e.p1.push_back(transformStamped.transform.translation.x); + o5e.p1.push_back(transformStamped.transform.translation.y); + o5e.p1.push_back(transformStamped.transform.translation.z); + o5e.p1[0] = 0.0; + o5f.p1.push_back(transformStamped.transform.translation.x); + o5f.p1.push_back(transformStamped.transform.translation.y); + o5f.p1.push_back(transformStamped.transform.translation.z); + o5f.p1[0] = 0.0; + o5g.p1.push_back(transformStamped.transform.translation.x); + o5g.p1.push_back(transformStamped.transform.translation.y); + o5g.p1.push_back(transformStamped.transform.translation.z); + o5g.p1[0] = 0.0; o5a.or2.w=transformStamped.transform.rotation.w; o5a.or2.x=transformStamped.transform.rotation.x; o5a.or2.y=transformStamped.transform.rotation.y; o5a.or2.z=transformStamped.transform.rotation.z; + o5b.or2.w=transformStamped.transform.rotation.w; + o5b.or2.x=transformStamped.transform.rotation.x; + o5b.or2.y=transformStamped.transform.rotation.y; + o5b.or2.z=transformStamped.transform.rotation.z; + + o5c.or2.w=transformStamped.transform.rotation.w; + o5c.or2.x=transformStamped.transform.rotation.x; + o5c.or2.y=transformStamped.transform.rotation.y; + o5c.or2.z=transformStamped.transform.rotation.z; + + o5d.or2.w=transformStamped.transform.rotation.w; + o5d.or2.x=transformStamped.transform.rotation.x; + o5d.or2.y=transformStamped.transform.rotation.y; + o5d.or2.z=transformStamped.transform.rotation.z; + + o5e.or2.w=transformStamped.transform.rotation.w; + o5e.or2.x=transformStamped.transform.rotation.x; + o5e.or2.y=transformStamped.transform.rotation.y; + o5e.or2.z=transformStamped.transform.rotation.z; + + o5f.or2.w=transformStamped.transform.rotation.w; + o5f.or2.x=transformStamped.transform.rotation.x; + o5f.or2.y=transformStamped.transform.rotation.y; + o5f.or2.z=transformStamped.transform.rotation.z; + + o5g.or2.w=transformStamped.transform.rotation.w; + o5g.or2.x=transformStamped.transform.rotation.x; + o5g.or2.y=transformStamped.transform.rotation.y; + o5g.or2.z=transformStamped.transform.rotation.z; + + } rate.sleep(); } return 0; -} +} \ No newline at end of file