Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • avaliente/gz_gep_tools
  • ostasse/gz_gep_tools
2 results
Show changes
Commits on Source (5)
Hide whitespace changes
Inline Side-by-side
Showing
with 418 additions and 149 deletions
CMakeLists.txt
View file @ efcc0fc1
......@@ -19,26 +19,22 @@ set(GZ_TRANSPORT_VER ${gz-transport13_VERSION_MAJOR})
find_package(gz-sim8 QUIET REQUIRED OPTIONAL_COMPONENTS log)
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
add_library(gz_transport_hw_interface SHARED
src/joint_state_interface.cc
src/control_over_gz.cc
src/perception_action_loop.cc
)
target_include_directories(gz_transport_hw_interface PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(gz_transport_hw_interface
gz-transport${GZ_TRANSPORT_VER}::core
gz-sim${GZ_SIM_VER}::core
${PROJECT_NAME}-msgs
)
add_library(
gz_transport_hw_interface SHARED
src/joint_state_interface.cc src/control_over_gz.cc
src/perception_action_loop.cc src/robots_data.cc)
target_include_directories(
gz_transport_hw_interface
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(
gz_transport_hw_interface gz-transport${GZ_TRANSPORT_VER}::core
gz-sim${GZ_SIM_VER}::core ${PROJECT_NAME}-msgs)
add_executable(control_loop ./tools/control_loop.cc)
target_include_directories(control_loop PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_include_directories(
control_loop PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(control_loop gz_transport_hw_interface)
add_subdirectory(plugin)
include/gz_gep_tools/control_over_gz.hh
View file @ efcc0fc1
......@@ -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,
......
include/gz_gep_tools/gz_gep_tools.hh
View file @ efcc0fc1
#include <gz_gep_tools/joint_state_interface.hh>
#include <gz_gep_tools/control_over_gz.hh>
#include <gz_gep_tools/perception_action_loop.hh>
#include <gz_gep_tools/robots_data.hh>
include/gz_gep_tools/joint_state_interface.hh
View file @ efcc0fc1
......@@ -36,11 +36,13 @@ class ControlJointValue {
double Cmd() const { return cmd_;}
double PosMes() const { return pos_mes_;}
double VelMes() const { return vel_mes_;}
double ForceMes() const { return force_mes_;}
double PosDes() const { return pos_des_;}
double VelDes() const { return vel_des_;}
void SetPosMes(double &pos_mes) { pos_mes_ = pos_mes;}
void SetVelMes(double &vel_mes) { vel_mes_ = vel_mes;}
void SetForceMes(double &force_mes) { force_mes_ = force_mes;}
void ComputeCmd();
......@@ -54,9 +56,9 @@ class ControlJointValue {
// Clamping i value to i_clamp
double i_clamp_;
// Measured quantities
double pos_mes_, vel_mes_;
double pos_mes_, vel_mes_, force_mes_;
/// Command
double cmd_;
......@@ -80,12 +82,13 @@ bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
const std::string &afilename);
/// Intermediate structure used to read information from Gazebo "joint_name/joint_state" topic.
///
///
class GZJointValues {
public:
/// Measured quantities
double pos_mes;
double vel_mes;
double force_mes;
/// Control
double force_ctrl;
};
......
include/gz_gep_tools/perception_action_loop.hh
View file @ efcc0fc1
#include <string>
#include "gz_gep_tools/joint_state_interface.hh"
......@@ -10,6 +11,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);
......@@ -18,23 +20,32 @@ class PerceptionActionLoop {
int InitGz();
/// Main loop
int MainLoop();
int MainLoop(unsigned long long int &);
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_;
/// Informmation from perception and the simulator
gz_transport_hw_tools::ControlOverGz control_over_gz_;
/// Time related variables
/// Time related to state variables
double state_gz_time_;
double pre_state_gz_time_;
/// Time related to gz status
double gz_time_;
double pre_gz_time_;
/// Local counter
unsigned long long int local_time_;
/// Pose3d when starting
std::vector<double> pose3d_;
/// Debug level
bool debug_level_;
};
......
include/gz_gep_tools/robots_data.hh 0 → 100644
View file @ efcc0fc1
#pragma once
#include <vector>
#include <gz_gep_tools/joint_state_interface.hh>
namespace gz_transport_hw_tools
{
/// Data related to TALOS
extern RobotCtrlJointInfos talos_ctrl_joint_infos;
extern std::vector<double> talos_pose3d;
/// Data related to H1_v2 full model
extern RobotCtrlJointInfos h1_v2_ctrl_joint_infos;
extern std::vector<double> h1_v2_pose3d;
extern RobotCtrlJointInfos h1_v2_woh_ctrl_joint_infos;
extern std::vector<double> h1_v2_woh_pose3d;
extern std::string talos_prefix_model_root;
extern std::string talos_prefix_world;
extern std::string h1_v2_prefix_model_root;
extern std::string h1_v2_prefix_world;
extern std::string h1_v2_woh_prefix_model_root;
extern std::string h1_v2_woh_prefix_world;
};
plugin/ApplyJointsForces.cc
View file @ efcc0fc1
......@@ -26,6 +26,7 @@
#include <gz/transport/Node.hh>
#include "gz/sim/components/JointForceCmd.hh"
#include "gz/sim/components/JointPosition.hh"
#include "gz/sim/components/Name.hh"
#include "gz/sim/components/Joint.hh"
#include "gz/sim/components/JointType.hh"
......@@ -66,6 +67,11 @@ class ActuatedJoint
class gz::sim::systems::ApplyJointsForcesPrivate
{
public: ApplyJointsForcesPrivate():
cmdForceInitialized(false)
{}
/// \brief Get a list of enabled, unique, 1-axis joints of the model. If no
/// joint names are specified in the plugin configuration, all valid 1-axis
/// joints are returned
......@@ -112,11 +118,13 @@ class gz::sim::systems::ApplyJointsForcesPrivate
const std::shared_ptr<const sdf::Element> &_sdf,
const std::string &_parameterName) const;
/// \brief Make sure that CmdForce has been initialized.
bool cmdForceInitialized;
};
//////////////////////////////////////////////////
ApplyJointsForces::ApplyJointsForces()
: dataPtr(std::make_unique<ApplyJointsForcesPrivate>())
: dataPtr(std::make_unique<ApplyJointsForcesPrivate>())
{
}
......@@ -252,8 +260,11 @@ void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
//! [findJoint]
if (an_actuated_joint->second.entity == kNullEntity)
{
gzwarn << "Did not find " << an_actuated_joint->first
<< " in ECM" << std::endl;
continue;
}
// Update joint force
//! [jointForceComponent]
......@@ -263,16 +274,22 @@ void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
std::lock_guard<std::mutex> lock(this->dataPtr->jointForceCmdMutex);
double force_to_apply;
if (!this->dataPtr->cmdForceInitialized)
force_to_apply = 0.0;
else
force_to_apply = an_actuated_joint->second.jointForceCmd;
//! [modifyComponent]
if (force == nullptr)
{
_ecm.CreateComponent(
an_actuated_joint->second.entity,
components::JointForceCmd({an_actuated_joint->second.jointForceCmd}));
components::JointForceCmd({force_to_apply}));
}
else
{
force->Data()[0] = an_actuated_joint->second.jointForceCmd;
force->Data()[0] += force_to_apply;
}
}
//! [modifyComponent]
......@@ -283,6 +300,7 @@ void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
void ApplyJointsForcesPrivate::CallbackCmdForce(
const gz::msgs::MapNamedJointsForces &mnjf_msg)
{
cmdForceInitialized = true;
std::lock_guard<std::mutex> lock(this->jointForceCmdMutex);
for ( auto mnjf_it = mnjf_msg.jointsforces().begin();
mnjf_it != mnjf_msg.jointsforces().end();
......
plugin/ApplyJointsForces.hh
View file @ efcc0fc1
......@@ -65,6 +65,7 @@ namespace systems
/// \brief Private data pointer
private: std::unique_ptr<ApplyJointsForcesPrivate> dataPtr;
};
}
}
......
src/control_over_gz.cc
View file @ efcc0fc1
......@@ -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") || modelName=="H1")
{
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();
......
src/joint_state_interface.cc
View file @ efcc0fc1
......@@ -88,7 +88,8 @@ bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
for(auto it_joint_info = a_rbt_ctrl_joint_infos.begin();
it_joint_info != a_rbt_ctrl_joint_infos.end();
it_joint_info++)
ofs_position_robot << it_joint_info->second.Cmd() << " ";
ofs_position_robot << it_joint_info->second.Cmd() << " "
<< it_joint_info->second.ForceMes() << " ";
ofs_position_robot << std::endl;
ofs_position_robot.close();
return true;
......@@ -97,12 +98,12 @@ bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
const std::string &afilename)
{
std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
std::ofstream ofs_position_robot(afilename.c_str());
unsigned int idx=1;
for(auto it_joint_info = a_rbt_ctrl_joint_infos.begin();
it_joint_info != a_rbt_ctrl_joint_infos.end();
it_joint_info++)
ofs_position_robot << it_joint_info->first << " ";
ofs_position_robot << std::endl;
ofs_position_robot << idx++ << " - " << it_joint_info->first << " " << std::endl;
ofs_position_robot.close();
return true;
}
......@@ -167,10 +168,11 @@ void JointStateInterface::CallbackJointState(
gz_robot_joints_.dict_joint_values[jointItr->name()].pos_mes = axis1.position();
gz_robot_joints_.dict_joint_values[jointItr->name()].vel_mes = axis1.velocity();
gz_robot_joints_.dict_joint_values[jointItr->name()].force_mes = axis1.force();
}
// if (debug_level_)
if (debug_level_)
{
std::cerr << "JointStateInterface::CallbackJointState: time: "
<< " " << gz_robot_joints_.time_sec_
......
src/perception_action_loop.cc
View file @ efcc0fc1
......@@ -30,11 +30,19 @@ 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),
joint_state_interface_(a_prefix_model_root, a_prefix_world, debug_level),
control_over_gz_(a_prefix_world, debug_level), debug_level_(debug_level)
control_over_gz_(a_prefix_world, debug_level),
state_gz_time_(0.0),
pre_state_gz_time_(0.0),
gz_time_(0.0),
pre_gz_time_(0.0),
local_time_(0),
pose3d_(a_pose3d),
debug_level_(debug_level)
{
joint_state_interface_.SetListOfJoints(a_robot_ctrl_joint_infos);
......@@ -57,8 +65,8 @@ int PerceptionActionLoop::InitGz()
if (!control_over_gz_.Reset()) {
std::cerr << "Reset failed" << std::endl;
}
state_gz_time_=0.0;
pre_state_gz_time_=control_over_gz_.GetSimTime();
gz_time_=0.0;
pre_gz_time_=control_over_gz_.GetSimTime();
using namespace std::chrono_literals;
// Wait 1 ms to perform reset.
......@@ -67,22 +75,35 @@ int PerceptionActionLoop::InitGz()
control_over_gz_.Step();
control_over_gz_.ReadWorldStateToInitECM();
while (pre_gz_time_==gz_time_) {
std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
gz_time_=control_over_gz_.GetSimTime();
}
// Second reset to set the robot state to a specific position.
gz_time_=0.0;
pre_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_gz_time_== gz_time_) {
std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
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();
/// Synchronize simulation wait for starting.
unsigned long int i=0;
while(std::isnan(pre_state_gz_time_) ||
(pre_state_gz_time_>0.001))
{ /// Wait (1ms)
while(std::isnan(pre_gz_time_) ||
(pre_gz_time_>0.001))
{ /// Wait (2ms)
std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
pre_state_gz_time_=control_over_gz_.GetSimTime();
pre_gz_time_=control_over_gz_.GetSimTime();
if ((i%10==0) && (i>1)){
control_over_gz_.Reset();
// std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
......@@ -91,7 +112,7 @@ int PerceptionActionLoop::InitGz()
if (i>100) {
std::cerr << "control_loop stuck in the waiting loop for starting."
<< std::endl
<< pre_state_gz_time_ << " "
<< pre_gz_time_ << " "
<< i << " "
<< std::endl;
return -1;
......@@ -102,7 +123,7 @@ int PerceptionActionLoop::InitGz()
return 0;
}
int PerceptionActionLoop::MainLoop()
int PerceptionActionLoop::MainLoop(unsigned long long int &duration)
{
unsigned long int internal_timer = 0;
std::string filename_pos("/tmp/position.dat");
......@@ -118,23 +139,34 @@ int PerceptionActionLoop::MainLoop()
/// CTRL-C loop
while (!g_terminatePub)
{
/// Sense
bool is_sim_ready = joint_state_interface_.GetPosVel(robot_ctrl_joint_infos_,
state_gz_time_);
/// Check if Gazebo time has been updated.
gz_time_ = control_over_gz_.GetSimTime();
/// By default sim is not ready
bool is_sim_ready = false;
/// Test if the simulator clock has been updated.
if (pre_gz_time_ < gz_time_)
/// Sense
is_sim_ready = joint_state_interface_.GetPosVel(robot_ctrl_joint_infos_,
state_gz_time_);
if (debug_level_)
std::cerr << "control_loop: "
<< is_sim_ready << " "
<< pre_gz_time_<< " "
<< gz_time_ << " "
<< pre_state_gz_time_<< " "
<< state_gz_time_ << " "
<< internal_timer
<< std::endl;
if ((is_sim_ready) && // If the simulation is ready
(pre_state_gz_time_<state_gz_time_) && // If the pre_state_gz_time_ is before state_gz_time_
(fabs(pre_state_gz_time_-state_gz_time_) < 0.005) // The update of state_gz_time_ might happen before
// reset and then putting state_gz_time_ to far ahead.
) {
(pre_gz_time_<gz_time_) && // If the pre_gz_time_ is before gz_time_
(fabs(pre_gz_time_-gz_time_) < 0.005) // The update of gz_time_ might happen before
// reset and then putting gz_time_ to far ahead.
)
{
{
/// Control computation.
......@@ -147,14 +179,17 @@ int PerceptionActionLoop::MainLoop()
}
/// Store Gazebo time
pre_state_gz_time_ = state_gz_time_;
pre_gz_time_ = gz_time_;
if (joint_state_interface_.SetCmd(robot_ctrl_joint_infos_))
{
SavePos(robot_ctrl_joint_infos_,filename_pos);
SavePosDes(robot_ctrl_joint_infos_,filename_pos_des);
SaveCmd(robot_ctrl_joint_infos_,filename_cmd);
if (true)
{
SavePos(robot_ctrl_joint_infos_,filename_pos);
SavePosDes(robot_ctrl_joint_infos_,filename_pos_des);
SaveCmd(robot_ctrl_joint_infos_,filename_cmd);
}
}
// Start simulation.
......@@ -165,14 +200,14 @@ int PerceptionActionLoop::MainLoop()
internal_timer=0;
/// Stop after 0.2 seconds.
if (local_time_>2000)
if (local_time_>duration)
break;
} else
{
// We have missed the Step for one sec.
if ((internal_timer>=1000) &&
(pre_state_gz_time_>=state_gz_time_))
(pre_gz_time_>=gz_time_))
{
std::cerr << "internal_timer: " << internal_timer << std::endl;
control_over_gz_.Step();
......@@ -182,7 +217,10 @@ int PerceptionActionLoop::MainLoop()
}
using namespace std::chrono_literals;
std::this_thread::sleep_for(1ms);
std::this_thread::sleep_for(2ms);
if ((local_time_%1000==0) && (internal_timer==0))
std::cout << "local_time:" << ((long double)local_time_)/1000.0 << " s " << std::endl
<< " internal_timer:" << internal_timer << std::endl;
internal_timer++;
}
// aControlOverGz.SendWorldControlState();
......
src/robots_data.cc 0 → 100644
View file @ efcc0fc1
#include <gz_gep_tools/robots_data.hh>
namespace gz_transport_hw_tools {
RobotCtrlJointInfos talos_ctrl_joint_infos {
{ "arm_left_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.25847 , 0.0)},
{ "arm_left_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.173046, 0.0)},
{ "arm_left_3_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.0002 , 0.0)},
{ "arm_left_4_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.525366, 0.0)},
{ "arm_left_5_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_left_6_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_left_7_joint", ControlJointValue( 100.0, 1.0 , 0.0, 3.0, 0.0, 0.0)},
{ "arm_right_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.25847 , 0.0)},
{ "arm_right_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.173046, 0.0)},
{ "arm_right_3_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, 0.0002 , 0.0)},
{ "arm_right_4_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.525366, 0.0)},
{ "arm_right_5_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_right_6_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_right_7_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "gripper_left_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0, 0.0, 0.0)},
{ "gripper_right_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0, 0.0, 0.0)},
{ "head_1_joint", ControlJointValue( 300.0, 0.1, 1.0, 5.0, 0.0, 0.0)},
{ "head_2_joint", ControlJointValue( 300.0, 0.1, 1.0, 1.5, 0.0, 0.0)},
{ "leg_left_1_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)},
{ "leg_left_2_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0, 0.0, 0.0)},
{ "leg_left_3_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)},
{ "leg_left_4_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.896082, 0.0)},
{ "leg_left_5_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)},
{ "leg_left_6_joint", ControlJointValue( 5000.0,20.0, 5.0, 9.0, 0.0, 0.0)},
{ "leg_right_1_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)},
{ "leg_right_2_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0, 0.0, 0.0)},
{ "leg_right_3_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)},
{ "leg_right_4_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.896082, 0.0)},
{ "leg_right_5_joint", ControlJointValue( 5000.0,20.0, 5.0, 9.0,-0.448041, 0.0)},
{ "leg_right_6_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)},
{ "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};
RobotCtrlJointInfos h1_v2_ctrl_joint_infos {
/// Left hand
{ "L_index_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_index_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_middle_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_middle_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_pinky_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_pinky_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_ring_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_ring_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_thumb_intermediate_joint", ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_thumb_proximal_yaw_joint", ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_thumb_distal_joint", ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "L_thumb_proximal_pitch_joint",ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
/// Right hand
{ "R_index_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_index_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_middle_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_middle_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_pinky_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_pinky_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_ring_intermediate_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_ring_proximal_joint", ControlJointValue( 100.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_thumb_intermediate_joint", ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_thumb_proximal_yaw_joint", ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_thumb_distal_joint", ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "R_thumb_proximal_pitch_joint",ControlJointValue( 150.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
/// Left arm
{ "left_shoulder_pitch_joint", ControlJointValue(10000.0, 0.01, 1.0, 14.0, 0.0, 0.0) },
{ "left_shoulder_roll_joint", ControlJointValue(10000.0, 0.01, 1.0, 14.0, 0.0, 0.0) },
{ "left_shoulder_yaw_joint", ControlJointValue(10000.0, 0.01, 1.0, 9.0, 0.0, 0.0) },
{ "left_elbow_joint", ControlJointValue( 5000.0, 0.7, 1.0, 14.0, 0.0, 0.0) },
{ "left_wrist_roll_joint", ControlJointValue( 50.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
{ "left_wrist_pitch_joint", ControlJointValue( 100.0, 4.0, 2.0, 14.0, 0.0, 0.0) },
{ "left_wrist_yaw_joint", ControlJointValue( 50.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
/// Right arm
{ "right_shoulder_pitch_joint",ControlJointValue(10000.0, 0.01, 1.0, 14.0, 0.0, 0.0) },
{ "right_shoulder_roll_joint", ControlJointValue(10000.0, 0.01, 1.0, 14.0, 0.0, 0.0) },
{ "right_shoulder_yaw_joint", ControlJointValue(10000.0, 0.01, 1.0, 9.0, 0.0, 0.0) },
{ "right_elbow_joint", ControlJointValue( 5000.0, 0.7, 1.0, 14.0, 0.0, 0.0) },
{ "right_wrist_roll_joint", ControlJointValue( 50.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
{ "right_wrist_pitch_joint", ControlJointValue( 50.0, 4.0, 2.0, 14.0, 0.0, 0.0) },
{ "right_wrist_yaw_joint", ControlJointValue( 50.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
/// Left leg
{ "left_hip_yaw_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0) },
{ "left_hip_pitch_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "left_hip_roll_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "left_knee_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.0, 0.0) },
{ "left_ankle_pitch_joint", ControlJointValue( 200.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
{ "left_ankle_roll_joint", ControlJointValue( 200.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
/// Right leg
{ "right_hip_yaw_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0) },
{ "right_hip_pitch_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "right_hip_roll_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "right_knee_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.0, 0.0) },
{ "right_ankle_pitch_joint", ControlJointValue( 200.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
{ "right_ankle_roll_joint", ControlJointValue( 200.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
/// Torso
{ "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};
RobotCtrlJointInfos h1_v2_woh_ctrl_joint_infos {
/// Left arm
{ "left_shoulder_pitch_joint", ControlJointValue(10000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_shoulder_roll_joint", ControlJointValue(1000.0, 2.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_shoulder_yaw_joint", ControlJointValue(10000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_elbow_joint", ControlJointValue(10000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_wrist_roll_joint", ControlJointValue( 5.0, 0.1, 0.0, 14.0, 0.0, 0.0) },
{ "left_wrist_pitch_joint", ControlJointValue( 500.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "left_wrist_yaw_joint", ControlJointValue( 50.0, 0.1, 0.0, 14.0, 0.0, 0.0) },
/// Right arm
{ "right_shoulder_pitch_joint",ControlJointValue(10000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "right_shoulder_roll_joint", ControlJointValue(1000.0, 2.0, 5.0, 14.0, 0.0, 0.0) },
{ "right_shoulder_yaw_joint", ControlJointValue(10000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "right_elbow_joint", ControlJointValue(10000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "right_wrist_roll_joint", ControlJointValue( 5.0, 0.1, 0.0, 14.0, 0.0, 0.0) },
{ "right_wrist_pitch_joint", ControlJointValue( 500.0, 0.0, 0.0, 14.0, 0.0, 0.0) },
{ "right_wrist_yaw_joint", ControlJointValue( 50.0, 0.1, 0.0, 14.0, 0.0, 0.0) },
/// Left leg
{ "left_hip_yaw_joint", ControlJointValue(10000.0,20.0, 5.0, 7.0, 0.0, 0.0) },
{ "left_hip_pitch_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "left_hip_roll_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "left_knee_joint", ControlJointValue( 500.0, 2.0, 1.0, 7.0, 0.0, 0.0) },
{ "left_ankle_pitch_joint", ControlJointValue( 400.0, 2.0, 1.0, 7.0, 0.0, 0.0) },
{ "left_ankle_roll_joint", ControlJointValue( 40.0, 1.0, 0.0, 7.0, 0.0, 0.0) },
/// Right leg
{ "right_hip_yaw_joint", ControlJointValue(10000.0,20.0, 5.0, 7.0, 0.0, 0.0) },
{ "right_hip_pitch_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "right_hip_roll_joint", ControlJointValue(10000.0,20.0, 1.0, 14.0, 0.0, 0.0) },
{ "right_knee_joint", ControlJointValue( 500.0, 2.0, 1.0, 7.0, 0.0, 0.0) },
{ "right_ankle_pitch_joint", ControlJointValue( 400.0, 2.0, 1.0, 7.0, 0.0, 0.0) },
{ "right_ankle_roll_joint", ControlJointValue( 40.0, 1.0, 0.0, 7.0, 0.0, 0.0) },
/// Torso
{ "torso_joint", ControlJointValue(10000.0,20.0, 1.0, 10.0, 0.0, 0.0) },
};
std::vector<double> h1_v2_woh_pose3d {0.0 , 0.0, 1.02, 0.0, 0.0, 0.0};
std::string talos_prefix_model_root("/model/Pyrene/");
std::string talos_prefix_world("/world/empty_talos_gz");
std::string h1_v2_prefix_model_root("/model/H1/");
std::string h1_v2_prefix_world("/world/empty_h1_2_gz");
std::string h1_v2_woh_prefix_model_root("/model/H1/");
std::string h1_v2_woh_prefix_world("/world/empty_h1_2_gz");
};
tools/control_loop.cc
View file @ efcc0fc1
......@@ -30,7 +30,6 @@
#include <gz/transport.hh>
#include <gz_gep_tools/gz_gep_tools.hh>
#include <gz_gep_tools/perception_action_loop.hh>
using namespace gz_transport_hw_tools;
......@@ -41,111 +40,58 @@ int main(int argc, char **argv)
int choice = 0;
if (argc!=2) {
std::cerr << argv[0] << " h|t" << std::endl
std::cerr << argv[0] << " h|t|w" << std::endl
<< " h : for the H1 robot (default)" << std::endl
<< " w : for the H1 robot without hands" << std::endl
<< " t : for the TALOS robot" << std::endl;
return -1;
}
else if (std::string(argv[1])==std::string("t"))
else if (std::string(argv[1])==std::string("w"))
choice = 1;
else if (std::string(argv[1])==std::string("t"))
choice = 2;
std::cout << "Choice :" << choice << std::endl;
// Create a transport node and advertise a topic.
gz::transport::Node node;
RobotCtrlJointInfos talos_ctrl_joint_infos {
{ "arm_left_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.25847 , 0.0)},
{ "arm_left_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.173046, 0.0)},
{ "arm_left_3_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.0002 , 0.0)},
{ "arm_left_4_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.525366, 0.0)},
{ "arm_left_5_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_left_6_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_left_7_joint", ControlJointValue( 100.0, 1.0 , 0.0, 3.0, 0.0, 0.0)},
{ "arm_right_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.25847 , 0.0)},
{ "arm_right_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.173046, 0.0)},
{ "arm_right_3_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, 0.0002 , 0.0)},
{ "arm_right_4_joint", ControlJointValue( 5000.0, 0.0 , 1.0, 9.0, -0.525366, 0.0)},
{ "arm_right_5_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_right_6_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "arm_right_7_joint", ControlJointValue( 500.0, 1.0 , 0.1, 5.0, 0.0, 0.0)},
{ "gripper_left_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0, 0.0, 0.0)},
{ "gripper_right_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0, 0.0, 0.0)},
{ "head_1_joint", ControlJointValue( 300.0, 0.1, 1.0, 5.0, 0.0, 0.0)},
{ "head_2_joint", ControlJointValue( 300.0, 0.1, 1.0, 1.5, 0.0, 0.0)},
{ "leg_left_1_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)},
{ "leg_left_2_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0, 0.0, 0.0)},
{ "leg_left_3_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)},
{ "leg_left_4_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.896082, 0.0)},
{ "leg_left_5_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)},
{ "leg_left_6_joint", ControlJointValue( 5000.0,20.0, 5.0, 9.0, 0.0, 0.0)},
{ "leg_right_1_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)},
{ "leg_right_2_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0, 0.0, 0.0)},
{ "leg_right_3_joint", ControlJointValue( 5000.0,20.0, 5.0, 14.0,-0.448041, 0.0)},
{ "leg_right_4_joint", ControlJointValue( 5000.0,20.0, 5.0, 25.0, 0.896082, 0.0)},
{ "leg_right_5_joint", ControlJointValue( 5000.0,20.0, 5.0, 9.0,-0.448041, 0.0)},
{ "leg_right_6_joint", ControlJointValue( 5000.0,20.0, 5.0, 7.0, 0.0, 0.0)},
{ "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)}
};
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) },
{ "left_shoulder_roll_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, 0.0, 0.0) },
{ "left_shoulder_yaw_joint", ControlJointValue( 5000.0, 0.0, 1.0, 9.0, 0.0, 0.0) },
{ "left_elbow_joint", ControlJointValue( 5000.0, 0.0, 1.0, 9.0, 0.0, 0.0) },
{ "left_wrist_roll_joint", ControlJointValue( 500.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
{ "left_wrist_pitch_joint", ControlJointValue( 500.0, 1.0, 0.1, 5.0, 0.0, 0.0) },
{ "left_wrist_yaw_joint", ControlJointValue( 500.0, 1.0, 0.1, 3.0, 0.0, 0.0) },
{ "right_shoulder_pitch_joint",ControlJointValue(10000.0, 0.01, 1.0, 0.0, 0.0, 0.0) },
{ "right_shoulder_roll_joint", ControlJointValue(10000.0, 0.01, 1.0, 0.0, 0.0, 0.0) },
{ "right_shoulder_yaw_joint", ControlJointValue( 5000.0, 0.0, 1.0, 0.0, 0.0, 0.0) },
{ "right_elbow_joint", ControlJointValue( 5000.0, 0.0, 1.0, 0.0, 0.0, 0.0) },
{ "right_wrist_roll_joint", ControlJointValue( 500.0, 1.0, 0.1, 0.0, 0.0, 0.0) },
{ "right_wrist_pitch_joint", ControlJointValue( 500.0, 1.0, 0.1, 0.0, 0.0, 0.0) },
{ "right_wrist_yaw_joint", ControlJointValue( 500.0, 1.0, 0.1, 0.0, 0.0, 0.0) },
{ "left_hip_yaw_joint", ControlJointValue( 5000.0, 20.0, 5.0, 7.0, 0.0, 0.0) },
{ "left_hip_pitch_joint", ControlJointValue( 5000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_hip_roll_joint", ControlJointValue( 5000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_knee_joint", ControlJointValue( 5000.0, 20.0, 5.0, 25.0, 0.0, 0.0) },
{ "left_ankle_pitch_joint", ControlJointValue( 5000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "left_ankle_roll_joint", ControlJointValue( 5000.0, 20.0, 5.0, 9.0, 0.0, 0.0) },
{ "right_hip_yaw_joint", ControlJointValue( 5000.0, 20.0, 5.0, 7.0, 0.0, 0.0) },
{ "right_hip_pitch_joint", ControlJointValue( 5000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "right_hip_roll_joint", ControlJointValue( 5000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "right_knee_joint", ControlJointValue( 5000.0, 20.0, 5.0, 25.0, 0.0, 0.0) },
{ "right_ankle_pitch_joint", ControlJointValue( 5000.0, 20.0, 5.0, 14.0, 0.0, 0.0) },
{ "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::string talos_prefix_model_root("/model/Pyrene/");
std::string talos_prefix_world("/world/empty_talos_gz");
std::string h1_v2_prefix_model_root("/model/H1/");
std::string h1_v2_prefix_world("/world/empty_h1_2_gz");
std::string & a_prefix_model_root = ( choice ==0 ) ?
h1_v2_prefix_model_root : talos_prefix_model_root;
std::string & a_prefix_world = (choice == 0) ?
h1_v2_prefix_world: talos_prefix_world;
std::string & a_prefix_model_root = gz_transport_hw_tools::h1_v2_prefix_model_root;
std::string & a_prefix_world = gz_transport_hw_tools::h1_v2_prefix_world;
std::vector<double> & a_pose3d = gz_transport_hw_tools::h1_v2_pose3d;
gz_transport_hw_tools::RobotCtrlJointInfos &
a_robot_ctrl_joint_infos = (choice==0) ?
h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ;
a_robot_ctrl_joint_infos = gz_transport_hw_tools::h1_v2_ctrl_joint_infos;
switch (choice) {
case (1):
a_prefix_model_root = gz_transport_hw_tools::h1_v2_woh_prefix_model_root;
a_prefix_world = gz_transport_hw_tools::h1_v2_woh_prefix_world;
a_pose3d = gz_transport_hw_tools::h1_v2_woh_pose3d;
a_robot_ctrl_joint_infos = gz_transport_hw_tools::h1_v2_woh_ctrl_joint_infos ;
break;
case(2) :
a_prefix_model_root = gz_transport_hw_tools::talos_prefix_model_root;
a_prefix_world = gz_transport_hw_tools::talos_prefix_world;
a_pose3d = gz_transport_hw_tools::talos_pose3d;
a_robot_ctrl_joint_infos = gz_transport_hw_tools::talos_ctrl_joint_infos ;
break;
}
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();
a_pal.MainLoop();
unsigned long long int duration= 20000;
a_pal.MainLoop(duration);
return 0;
}
......
tools/display_pos.gp 0 → 100644
View file @ efcc0fc1
p="/tmp/position.dat"
set output 'left_hand.png'
set terminal png
set title "Left Hand"
plot p u 1 w l t "LIP", p u 2 w l t "LMP", p u 3 w l t "LPP", p u 4 w l t "LRP", p u 5 w l t "LTPP", p u 6 w l t "LTPY", p u 7 w l t "RIP", p u 8 w l t "RMP", p u 9 w l t "RPP", p u 10 w l t "RRP", p u 11 w l t "RTPP", p u 12 w l t "RTPY"
set output 'right_hand.png'
set title "Right Hand"
plot p u 13 w l t "LIP", p u 14 w l t "LMP", p u 15 w l t "LPP", p u 16 w l t "LRP", p u 17 w l t "LTPP", p u 18 w l t "LTPY", p u 19 w l t "RIP", p u 20 w l t "RMP", p u 21 w l t "RPP", p u 22 w l t "RRP", p u 23 w l t "RTPP", p u 24 w l t "RTPY"
set output 'left_leg.png'
set title "Left Leg"
plot p u 28 w l t "lhp", p u 29 w l t "lhr", p u 30 w l t "lhy", p u 31 w l t "lk", p u 25 w l t "lap", p u 26 w l t "lar"
set output 'right_leg.png'
set title "Right Leg"
plot p u 41 w l t "rhp", p u 42 w l t "rhr", p u 43 w l t "rhy", p u 44 w l t "rk", p u 38 w l t "rap", p u 39 w l t "rar"
set output 'left_arm.png'
set title "Left Arm"
plot p u 27 w l t "le", p u 32 w l t "lsp", p u 33 w l t "lsr", p u 34 w l t "lsy", p u 35 w l t "lwp", p u 36 w l t "lwr", p u 37 w l t "lwy"
set output 'right_arm.png'
set title "Right Arm"
plot p u 40 w l t "re", p u 45 w l t "rsp", p u 46 w l t "rsr", p u 47 w l t "rsy", p u 48 w l t "rwp", p u 49 w l t "rwr", p u 50 w l t "rwy"
unset multiplot
tools/display_pos_wo_hands.gp 0 → 100644
View file @ efcc0fc1
p="/tmp/position.dat"
set terminal png
set output 'left_leg.png'
set title "Left Leg"
plot p u 4 w l t "lhp", p u 5 w l t "lhr", p u 6 w l t "lhy", p u 7 w l t "lk", p u 1 w l t "lap", p u 2 w l t "lar"
set output 'right_leg.png'
set title "Right Leg"
plot p u 17 w l t "rhp", p u 18 w l t "rhr", p u 19 w l t "rhy", p u 20 w l t "rk", p u 14 w l t "rap", p u 15 w l t "rar"
set output 'left_arm.png'
set title "Left Arm"
plot p u 3 w l t "le", p u 8 w l t "lsp", p u 9 w l t "lsr", p u 10 w l t "lsy", p u 11 w l t "lwp", p u 12 w l t "lwr", p u 13 w l t "lwy"
set output 'right_arm.png'
set title "Right Arm"
plot p u 16 w l t "re", p u 21 w l t "rsp", p u 22 w l t "rsr", p u 23 w l t "rsy", p u 24 w l t "rwp", p u 25 w l t "rwr", p u 26 w l t "rwy"
unset multiplot