Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

repaired dex3 example and added CmakeList which has execuable program #13

Merged
merged 1 commit into from
Jan 21, 2025
Merged

repaired dex3 example and added CmakeList which has execuable program #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions example/g1/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@ target_link_libraries(g1_ankle_swing_example unitree_sdk2)
add_executable(g1_audio_client_example audio/g1_audio_client_example.cpp)
target_link_libraries(g1_audio_client_example unitree_sdk2)

add_executable(g1_dex3_example dex3/g1_dex3_example.cpp)
target_link_libraries(g1_dex3_example unitree_sdk2)

find_package(yaml-cpp QUIET)
if(yaml-cpp_FOUND)
if (${yaml-cpp_VERSION} VERSION_GREATER_EQUAL "0.6")
Expand Down
63 changes: 34 additions & 29 deletions example/g1/dex3/dex3_subscribe.cpp → example/g1/dex3/g1_dex3_example.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#include <chrono>
#include <thread>
#include <unitree/hg_idl/HandState_.hpp> //replace your sdk path
#include <unitree/hg_idl/HandCmd_.hpp> //replace your sdk path
#include <unitree/idl/hg/HandState_.hpp> //replace your sdk path
#include <unitree/idl/hg/HandCmd_.hpp> //replace your sdk path
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <iostream>
Expand All @@ -22,15 +22,13 @@ enum State {
PRINT
};

// �ο�urdf�����λ
const float maxTorqueLimits_left[7]= { 1.05 , 1.05 , 1.75 , 0 , 0 , 0 , 0 }; // set max motor value
const float minTorqueLimits_left[7]= { -1.05 , -0.724 , 0 , -1.57 , -1.75 , -1.57 ,-1.75};
const float maxTorqueLimits_right[7]= { 1.05 , 0.742 , 0 , 1.57 , 1.75 , 1.57 , 1.75};
const float minTorqueLimits_right[7]= { -1.05 , -1.05 , -1.75, 0 , 0 , 0 ,0 };


//State currentState = INIT;
// set URDF Limits
const float maxLimits_left[7]= { 1.05 , 1.05 , 1.75 , 0 , 0 , 0 , 0 }; // set max motor value
const float minLimits_left[7]= { -1.05 , -0.724 , 0 , -1.57 , -1.75 , -1.57 ,-1.75};
const float maxLimits_right[7]= { 1.05 , 0.742 , 0 , 1.57 , 1.75 , 1.57 , 1.75};
const float minLimits_right[7]= { -1.05 , -1.05 , -1.75, 0 , 0 , 0 ,0 };

// Initing the dds configuration
std::string dds_namespace = "rt/dex3/left";
std::string sub_namespace = "rt/dex3/left/state";
unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::HandCmd_> handcmd_publisher;
Expand All @@ -50,7 +48,7 @@ typedef struct {
uint8_t timeout: 1;
} RIS_Mode_t;

// stateToString ��������
// stateToString Method
const char* stateToString(State state) {
switch (state) {
case INIT: return "INIT";
Expand All @@ -62,7 +60,7 @@ const char* stateToString(State state) {
}
}


// Monitor user's input
char getNonBlockingInput() {
struct termios oldt, newt;
char ch;
Expand All @@ -75,16 +73,14 @@ char getNonBlockingInput() {
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

ch = getchar(); // ��ȡ�����ַ�
ch = getchar();

tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);

return ch;
}



void userInputThread() {
while (true) {
char ch = getNonBlockingInput();
Expand All @@ -105,11 +101,12 @@ void userInputThread() {
}
}

// this method can send kp and kd to motors
void rotateMotors(bool isLeftHand) {
static int _count = 1;
static int dir = 1;
const float* maxTorqueLimits = isLeftHand ? maxTorqueLimits_left : maxTorqueLimits_right;
const float* minTorqueLimits = isLeftHand ? minTorqueLimits_left : minTorqueLimits_right;
const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;

for (int i = 0; i < MOTOR_MAX; i++) {
RIS_Mode_t ris_mode;
Expand All @@ -127,8 +124,8 @@ void rotateMotors(bool isLeftHand) {
msg.motor_cmd()[i].kd(0.1);


float range = maxTorqueLimits[i] - minTorqueLimits[i];
float mid = (maxTorqueLimits[i] + minTorqueLimits[i]) / 2.0;
float range = maxLimits[i] - minLimits[i];
float mid = (maxLimits[i] + minLimits[i]) / 2.0;
float amplitude = range / 2.0;
float q = mid + amplitude * sin(_count / 20000.0 * M_PI);

Expand All @@ -149,15 +146,16 @@ void rotateMotors(bool isLeftHand) {
usleep(100);
}

// this method can send static position to motors
void gripHand(bool isLeftHand) {

const float* maxTorqueLimits = isLeftHand ? maxTorqueLimits_left : maxTorqueLimits_right;
const float* minTorqueLimits = isLeftHand ? minTorqueLimits_left : minTorqueLimits_right;
const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;

for (int i = 0; i < MOTOR_MAX; i++) {
RIS_Mode_t ris_mode;
ris_mode.id = i;
ris_mode.status = 0x01; /
ris_mode.status = 0x01;


uint8_t mode = 0;
Expand All @@ -168,7 +166,7 @@ void gripHand(bool isLeftHand) {
msg.motor_cmd()[i].tau(0);


float mid = (maxTorqueLimits[i] + minTorqueLimits[i]) / 2.0;
float mid = (maxLimits[i] + minLimits[i]) / 2.0;


msg.motor_cmd()[i].q(mid);
Expand All @@ -182,6 +180,7 @@ void gripHand(bool isLeftHand) {
usleep(1000000);
}

// this method can send dynamic position to motors
void stopMotors() {
for (int i = 0; i < MOTOR_MAX; i++) {
RIS_Mode_t ris_mode;
Expand All @@ -205,17 +204,17 @@ void stopMotors() {
usleep(1000000);
}


// this method can subscribe dds and show the position for now
void printState(bool isLeftHand){
Eigen::Matrix<float, 7, 1> q;

const float* maxTorqueLimits = isLeftHand ? maxTorqueLimits_left : maxTorqueLimits_right;
const float* minTorqueLimits = isLeftHand ? minTorqueLimits_left : minTorqueLimits_right;
const float* maxLimits = isLeftHand ? maxLimits_left : maxLimits_right;
const float* minLimits = isLeftHand ? minLimits_left : minLimits_right;
for(int i = 0; i < 7; i++)
{
q(i) = state.motor_state()[i].q();

q(i) = (q(i) - minTorqueLimits[i] ) / (maxTorqueLimits[i] - minTorqueLimits[i]);
q(i) = (q(i) - minLimits[i] ) / (maxLimits[i] - minLimits[i]);
q(i) = std::clamp(q(i), 0.0f, 1.0f);
}
std::cout << "\033[2J\033[H";
Expand All @@ -240,7 +239,8 @@ void StateHandler(const void *message) {



int main() {
int main(int argc, const char** argv)
{
std::cout << " --- Unitree Robotics --- \n";
std::cout << " Dex3 Hand Example \n\n";
std::string input;
Expand All @@ -260,7 +260,12 @@ int main() {
return -1;
}

unitree::robot::ChannelFactory::Instance()->Init(0);
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
handcmd_publisher.reset(new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::HandCmd_>(dds_namespace + "/cmd"));
handstate_subscriber.reset(new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::HandState_>(sub_namespace));
handcmd_publisher->InitChannel();
Expand Down
Loading