Skip to content

Commit

Permalink
add MotionSwitcherClient for humanoid examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@xiaoliangstd
xiaoliangstd committed Oct 31, 2024
1 parent c02ac39 commit 8fe9d6e
Show file tree
Hide file tree
Showing 3 changed files with 189 additions and 0 deletions.
65 changes: 65 additions & 0 deletions example/g1/low_level/g1_ankle_swing_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,9 @@
#include <unitree/idl/hg/LowCmd_.hpp>
#include <unitree/idl/hg/LowState_.hpp>

#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
using namespace unitree::robot::b2;

static const std::string HG_CMD_TOPIC = "rt/lowcmd";
static const std::string HG_STATE_TOPIC = "rt/lowstate";

Expand Down Expand Up @@ -157,6 +160,8 @@ class G1Example {
ChannelSubscriberPtr<LowState_> lowstate_subscriber_;
ThreadPtr command_writer_ptr_, control_thread_ptr_;

std::shared_ptr<MotionSwitcherClient> msc;

public:
G1Example(std::string networkInterface)
: time_(0.0),
Expand All @@ -165,6 +170,24 @@ class G1Example {
mode_pr_(Mode::PR),
mode_machine_(0) {
ChannelFactory::Instance()->Init(0, networkInterface);

msc.reset(new MotionSwitcherClient());
msc->SetTimeout(5.0F);
msc->Init();

/*Shut down motion control-related service*/
while(queryMotionStatus())
{
std::cout << "Try to deactivate the motion control-related service." << std::endl;
int32_t ret = msc->ReleaseMode();
if (ret == 0) {
std::cout << "ReleaseMode succeeded." << std::endl;
} else {
std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
}
sleep(5);
}

// create publisher
lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
lowcmd_publisher_->InitChannel();
Expand Down Expand Up @@ -290,6 +313,48 @@ class G1Example {
motor_command_buffer_.SetData(motor_command_tmp);
}
}


std::string queryServiceName(std::string form,std::string name)
{
if(form == "0")
{
if(name == "normal" ) return "sport_mode";
if(name == "ai" ) return "ai_sport";
if(name == "advanced" ) return "advanced_sport";
}
else
{
if(name == "ai-w" ) return "wheeled_sport(go2W)";
if(name == "normal-w" ) return "wheeled_sport(b2W)";
}
return "";
}

int queryMotionStatus()
{
std::string robotForm,motionName;
int motionStatus;
int32_t ret = msc->CheckMode(robotForm,motionName);
if (ret == 0) {
std::cout << "CheckMode succeeded." << std::endl;
} else {
std::cout << "CheckMode failed. Error code: " << ret << std::endl;
}
if(motionName.empty())
{
std::cout << "The motion control-related service is deactivated." << std::endl;
motionStatus = 0;
}
else
{
std::string serviceName = queryServiceName(robotForm,motionName);
std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
motionStatus = 1;
}
return motionStatus;
}

};

int main(int argc, char const *argv[]) {
Expand Down
62 changes: 62 additions & 0 deletions example/g1/low_level/g1_dual_arm_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@
#include <unitree/idl/hg/LowCmd_.hpp>
#include <unitree/idl/hg/LowState_.hpp>

#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
using namespace unitree::robot::b2;

static const std::string HG_CMD_TOPIC = "rt/lowcmd";
static const std::string HG_STATE_TOPIC = "rt/lowstate";

Expand Down Expand Up @@ -161,6 +164,8 @@ class G1Example {
ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_;
ThreadPtr command_writer_ptr_, control_thread_ptr_;

std::shared_ptr<MotionSwitcherClient> msc;

public:
G1Example(std::string networkInterface)
: time_(0.0),
Expand All @@ -170,6 +175,23 @@ class G1Example {
mode_machine_(0) {
ChannelFactory::Instance()->Init(0, networkInterface);

msc.reset(new MotionSwitcherClient());
msc->SetTimeout(5.0F);
msc->Init();

/*Shut down motion control-related service*/
while(queryMotionStatus())
{
std::cout << "Try to deactivate the motion control-related service." << std::endl;
int32_t ret = msc->ReleaseMode();
if (ret == 0) {
std::cout << "ReleaseMode succeeded." << std::endl;
} else {
std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
}
sleep(5);
}

loadBehaviorLibrary("motion");

// create publisher
Expand Down Expand Up @@ -328,6 +350,46 @@ class G1Example {
motor_command_buffer_.SetData(motor_command_tmp);
}
}

std::string queryServiceName(std::string form,std::string name)
{
if(form == "0")
{
if(name == "normal" ) return "sport_mode";
if(name == "ai" ) return "ai_sport";
if(name == "advanced" ) return "advanced_sport";
}
else
{
if(name == "ai-w" ) return "wheeled_sport(go2W)";
if(name == "normal-w" ) return "wheeled_sport(b2W)";
}
return "";
}

int queryMotionStatus()
{
std::string robotForm,motionName;
int motionStatus;
int32_t ret = msc->CheckMode(robotForm,motionName);
if (ret == 0) {
std::cout << "CheckMode succeeded." << std::endl;
} else {
std::cout << "CheckMode failed. Error code: " << ret << std::endl;
}
if(motionName.empty())
{
std::cout << "The motion control-related service is deactivated." << std::endl;
motionStatus = 0;
}
else
{
std::string serviceName = queryServiceName(robotForm,motionName);
std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
motionStatus = 1;
}
return motionStatus;
}
};

int main(int argc, char const *argv[]) {
Expand Down
62 changes: 62 additions & 0 deletions example/h1/low_level/humanoid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@
#include "data_buffer.hpp"
#include "motors.hpp"

#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
using namespace unitree::robot::b2;

static const std::string kTopicLowCommand = "rt/lowcmd";
static const std::string kTopicLowState = "rt/lowstate";

Expand All @@ -23,6 +26,23 @@ class HumanoidExample {
unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface);
std::cout << "Initialize channel factory." << std::endl;

msc.reset(new MotionSwitcherClient());
msc->SetTimeout(5.0F);
msc->Init();

/*Shut down motion control-related service*/
while(queryMotionStatus())
{
std::cout << "Try to deactivate the motion control-related service." << std::endl;
int32_t ret = msc->ReleaseMode();
if (ret == 0) {
std::cout << "ReleaseMode succeeded." << std::endl;
} else {
std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
}
sleep(5);
}

lowcmd_publisher_.reset(
new unitree::robot::ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(
kTopicLowCommand));
Expand Down Expand Up @@ -167,6 +187,46 @@ class HumanoidExample {
motor_index == JointIndex::kLeftElbow;
}

std::string queryServiceName(std::string form,std::string name)
{
if(form == "0")
{
if(name == "normal" ) return "sport_mode";
if(name == "ai" ) return "ai_sport";
if(name == "advanced" ) return "advanced_sport";
}
else
{
if(name == "ai-w" ) return "wheeled_sport(go2W)";
if(name == "normal-w" ) return "wheeled_sport(b2W)";
}
return "";
}

int queryMotionStatus()
{
std::string robotForm,motionName;
int motionStatus;
int32_t ret = msc->CheckMode(robotForm,motionName);
if (ret == 0) {
std::cout << "CheckMode succeeded." << std::endl;
} else {
std::cout << "CheckMode failed. Error code: " << ret << std::endl;
}
if(motionName.empty())
{
std::cout << "The motion control-related service is deactivated." << std::endl;
motionStatus = 0;
}
else
{
std::string serviceName = queryServiceName(robotForm,motionName);
std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
motionStatus = 1;
}
return motionStatus;
}

unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_>
lowcmd_publisher_;
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_>
Expand All @@ -176,6 +236,8 @@ class HumanoidExample {
DataBuffer<MotorCommand> motor_command_buffer_;
DataBuffer<BaseState> base_state_buffer_;

std::shared_ptr<MotionSwitcherClient> msc;

// control params
float kp_low_ = 60.f;
float kp_high_ = 200.f;
Expand Down

0 comments on commit 8fe9d6e

Please sign in to comment.