Skip to content
Snippets Groups Projects
Commit 5227c75a authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add pose3D

parent 9c64d532
No related branches found
No related tags found
1 merge request!2Pose 3d
......@@ -37,7 +37,8 @@ class ControlOverGz {
double GetSimTime();
/// Send World Control state to Init ECM.
bool SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos);
bool SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos,
std::vector<double> &aPose3d);
/// Set Pose
bool SetPose(double x, double y, double z,
......
......@@ -10,6 +10,7 @@ class PerceptionActionLoop {
public:
// Default constructore
PerceptionActionLoop(gz_transport_hw_tools::RobotCtrlJointInfos & a_robot_ctrl_joint_infos,
std::vector<double> & a_pose3d,
std::string & a_prefix_model_root,
std::string & a_prefix_world,
bool debug_level);
......@@ -19,11 +20,11 @@ class PerceptionActionLoop {
/// Main loop
int MainLoop();
protected:
/// Information for control
gz_transport_hw_tools::RobotCtrlJointInfos & robot_ctrl_joint_infos_;
/// Informmation from perception and the simulator
gz_transport_hw_tools::JointStateInterface joint_state_interface_;
......@@ -35,6 +36,9 @@ class PerceptionActionLoop {
double pre_state_gz_time_;
unsigned long long int local_time_;
/// Pose3d when starting
std::vector<double> pose3d_;
/// Debug level
bool debug_level_;
};
......
......@@ -3,6 +3,7 @@
#include <cstdint>
#include <cstring>
#include <gz/math/Pose3.hh>
#include <gz/msgs/empty.pb.h>
#include <gz_gep_tools/control_over_gz.hh>
#include <gz/msgs.hh>
......@@ -11,8 +12,10 @@
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/components/Joint.hh>
#include <gz/sim/components/Link.hh>
#include <gz/sim/components/Model.hh>
#include <gz/sim/Joint.hh>
#include <gz/sim/Link.hh>
#include <gz/sim/Model.hh>
namespace gz_transport_hw_tools {
......@@ -89,6 +92,11 @@ bool ControlOverGz::Reset()
req_world_ctrl.set_pause(true);
/// Set the world reset inside the world control reset.
req_world_ctrl.set_allocated_reset(req_world_reset);
req_world_ctrl.set_multi_step(0);
req_world_ctrl.set_step (false);
req_world_ctrl.set_pause(true);
req_world_ctrl.set_seed(0);
/// Do request.
bool result;
if (!node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result)) {
......@@ -193,7 +201,8 @@ void ControlOverGz::DisplayJointValues()
}
}
bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos)
bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos,
std::vector<double> &aPose3d)
{
gz::msgs::WorldControlState req_world_ctrl_state;
gz::msgs::SerializedState * req_serialized_state;
......@@ -223,6 +232,24 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
}
}
auto mentities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Model());
for (auto it_Entity = mentities.begin();
it_Entity != mentities.end();
it_Entity++)
{
gz::sim::Model aModel(*it_Entity);
std::string modelName = aModel.Name(Robot_ECM_);
std::cout << "Model name:" << modelName << std::endl;
if (modelName=="Pyrene")
{
gz::math::Pose3d aPose;
aPose.Set( aPose3d[0], aPose3d[1], aPose3d[2],
aPose3d[3], aPose3d[4], aPose3d[5]);
aModel.SetWorldPoseCmd(Robot_ECM_,aPose);
}
}
/// Create the serialized state.
req_serialized_state = new gz::msgs::SerializedState();
......
......@@ -30,9 +30,11 @@ namespace gz_transport_hw_tools {
PerceptionActionLoop::PerceptionActionLoop(
gz_transport_hw_tools::RobotCtrlJointInfos &a_robot_ctrl_joint_infos,
std::vector<double> &a_pose3d,
std::string &a_prefix_model_root, std::string &a_prefix_world,
bool debug_level)
: robot_ctrl_joint_infos_(a_robot_ctrl_joint_infos),
pose3d_(a_pose3d),
joint_state_interface_(a_prefix_model_root, a_prefix_world, debug_level),
control_over_gz_(a_prefix_world, debug_level), debug_level_(debug_level)
{
......@@ -67,11 +69,24 @@ int PerceptionActionLoop::InitGz()
control_over_gz_.Step();
control_over_gz_.ReadWorldStateToInitECM();
while (pre_state_gz_time_==state_gz_time_) {
std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
state_gz_time_=control_over_gz_.GetSimTime();
}
// Second reset to set the robot state to a specific position.
state_gz_time_=0.0;
pre_state_gz_time_=control_over_gz_.GetSimTime();
if (!control_over_gz_.Reset()) {
std::cerr << "Reset failed" << std::endl;
}
control_over_gz_.SendWorldControlStateToInitECM(robot_ctrl_joint_infos_);
while (pre_state_gz_time_==state_gz_time_) {
std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
state_gz_time_=control_over_gz_.GetSimTime();
}
control_over_gz_.SendWorldControlStateToInitECM(robot_ctrl_joint_infos_,
pose3d_);
if (debug_level_)
control_over_gz_.DisplayLinkValues();
control_over_gz_.Step();
......
......@@ -85,6 +85,7 @@ int main(int argc, char **argv)
{ "torso_1_joint", ControlJointValue( 10000.0,10.0, 1.0, 10.0, 0.0, 0.0)},
{ "torso_2_joint", ControlJointValue( 10000.0,10.0, 1.0, 10.0, 0.0, 0.0)}
};
std::vector<double> talos_pose3d {0.0 , 0.0, 1.02, 0.0, 0.0, 0.0};
gz_transport_hw_tools::RobotCtrlJointInfos h1_2_ctrl_joint_infos {
{ "left_shoulder_pitch_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.0, 0.0) },
......@@ -115,6 +116,7 @@ int main(int argc, char **argv)
{ "right_ankle_roll_joint", ControlJointValue( 5000.0, 20.0, 5.0, 9.0, 0.0, 0.0) },
{ "torso_joint", ControlJointValue(10000.0, 20.0, 1.0, 10.0, 0.0, 0.0) },
};
std::vector<double> h1_v2_pose3d {0.0 , 0.0, 0.0, 0.0, 0.0, 0.0};
std::string talos_prefix_model_root("/model/Pyrene/");
......@@ -130,17 +132,21 @@ int main(int argc, char **argv)
std::string & a_prefix_world = (choice == 0) ?
h1_v2_prefix_world: talos_prefix_world;
std::vector<double> & a_pose3d = (choice ==0) ?
h1_v2_pose3d : talos_pose3d;
gz_transport_hw_tools::RobotCtrlJointInfos &
a_robot_ctrl_joint_infos = (choice==0) ?
h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ;
bool debug_level = true;
bool debug_level = false;
gz_transport_hw_tools::PerceptionActionLoop a_pal(a_robot_ctrl_joint_infos,
a_prefix_model_root,
a_prefix_world,
debug_level);
a_pose3d,
a_prefix_model_root,
a_prefix_world,
debug_level);
a_pal.InitGz();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment