Skip to content

Add support for controlling Z1 arm mounted on Aliengo quadruped (aliengoZ1 model) #7

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

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add support for controlling Z1 arm mounted on Aliengo quadruped (aliengoZ1 model) #7

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
39 changes: 22 additions & 17 deletions sim/IOROS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,16 +8,21 @@ void RosShutDown(int sig){
ros::shutdown();
}

IOROS::IOROS(){
IOROS::IOROS() : _rname("z1") {
std::cout << "The control interface for ROS Gazebo simulation" << std::endl;
hasGripper = false;

/* retrieve robot_name from ROS parameter server */
ros::param::get("/robot_name", _rname); // defaults to "z1" if parameter not found
std::cout << "robot_name: " << _rname << std::endl;

/* start subscriber */
_initRecv();
ros::AsyncSpinner subSpinner(1); // one threads
subSpinner.start();
usleep(300000); //wait for subscribers start
/* initialize publisher */
_initSend();
_initSend();

signal(SIGINT, RosShutDown);

Expand Down Expand Up @@ -65,23 +70,23 @@ void IOROS::_recvState(LowlevelState *state){
}

void IOROS::_initSend(){
_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint01_controller/command", 1);
_servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint02_controller/command", 1);
_servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint03_controller/command", 1);
_servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint04_controller/command", 1);
_servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint05_controller/command", 1);
_servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint06_controller/command", 1);
_servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/gripper_controller/command", 1);
_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint01_controller/command", 1);
_servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint02_controller/command", 1);
_servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint03_controller/command", 1);
_servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint04_controller/command", 1);
_servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint05_controller/command", 1);
_servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint06_controller/command", 1);
_servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/gripper_controller/command", 1);
}

void IOROS::_initRecv(){
_servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
_servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
_servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
_servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
_servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
_servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
_servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
_servo_sub[0] = _nm.subscribe("/" + _rname + "_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
_servo_sub[1] = _nm.subscribe("/" + _rname + "_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
_servo_sub[2] = _nm.subscribe("/" + _rname + "_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
_servo_sub[3] = _nm.subscribe("/" + _rname + "_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
_servo_sub[4] = _nm.subscribe("/" + _rname + "_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
_servo_sub[5] = _nm.subscribe("/" + _rname + "_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
_servo_sub[6] = _nm.subscribe("/" + _rname + "_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
}

void IOROS::_joint00Callback(const unitree_legged_msgs::MotorState& msg){
Expand Down Expand Up @@ -139,4 +144,4 @@ void IOROS::_gripperCallback(const unitree_legged_msgs::MotorState& msg){
_joint_state[6].dq = msg.dq;
_joint_state[6].ddq = msg.ddq;
_joint_state[6].tauEst = msg.tauEst;
}
}
3 changes: 2 additions & 1 deletion sim/IOROS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ class IOROS : public IOInterface{
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
private:
ros::NodeHandle _nm;
std::string _rname; // robot_name retrieved from ROS parameter server
ros::Subscriber _servo_sub[7];
ros::Publisher _servo_pub[7];
unitree_legged_msgs::MotorState _joint_state[7];
Expand All @@ -31,4 +32,4 @@ class IOROS : public IOInterface{
void _gripperCallback(const unitree_legged_msgs::MotorState& msg);
};

#endif // IOROS_H
#endif // IOROS_H