Skip to content

Commit

Permalink
Merge pull request #13 from Julian-Won/main
Browse files Browse the repository at this point in the history 
repaired dex3 example and added CmakeList which has execuable program
  • Loading branch information
@Julian-Won
Julian-Won authored Jan 21, 2025
2 parents 197b720 + f07a541 commit 45cac25
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 29 deletions.
3 changes: 3 additions & 0 deletions example/g1/CMakeLists.txt
View file
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
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

0 comments on commit 45cac25

Please sign in to comment.