From fa9b011d74356e9eade03f67c646327c207cc826 Mon Sep 17 00:00:00 2001
From: Agnel Wang <59766821+Agnel-Wang@users.noreply.github.com>
Date: Tue, 27 Jun 2023 16:23:00 +0800
Subject: [PATCH] modify z1_urdf
---
README.md | 6 +-
.../aliengoZ1_description/xacro/robot.xacro | 6 +-
robots/z1_description/xacro/const.xacro | 16 +--
robots/z1_description/xacro/gazebo.xacro | 3 -
robots/z1_description/xacro/robot.xacro | 12 +-
robots/z1_description/xacro/z1.urdf | 35 ++---
z1_controller/CMakeLists.txt | 42 ------
z1_controller/include/unitreeArm.h | 37 ------
z1_controller/package.xml | 34 -----
z1_controller/src/basic.cpp | 17 ---
z1_controller/src/unitreeArm.cpp | 123 ------------------
11 files changed, 28 insertions(+), 303 deletions(-)
delete mode 100644 z1_controller/CMakeLists.txt
delete mode 100644 z1_controller/include/unitreeArm.h
delete mode 100644 z1_controller/package.xml
delete mode 100644 z1_controller/src/basic.cpp
delete mode 100644 z1_controller/src/unitreeArm.cpp
diff --git a/README.md b/README.md
index a8a9ddc..9f0d559 100644
--- a/README.md
+++ b/README.md
@@ -85,8 +85,4 @@ roslaunch unitree_gazebo z1.launch
```
After launching the gazebo simulation, you can start to control the z1 robot by z1_sdk.
-see [z1_documentation](dev-z1.unitree.com/5-sdk/run.html)
-You can also send ros commands directly to z1 robot:
-```
-rosrun z1_controller unitree_move_z1
-```
+see [z1_documentation](https://dev-z1.unitree.com)
\ No newline at end of file
diff --git a/robots/aliengoZ1_description/xacro/robot.xacro b/robots/aliengoZ1_description/xacro/robot.xacro
index 0739f0a..7a2823e 100644
--- a/robots/aliengoZ1_description/xacro/robot.xacro
+++ b/robots/aliengoZ1_description/xacro/robot.xacro
@@ -219,7 +219,7 @@
-
+
@@ -249,9 +249,9 @@
-
+
-
+
diff --git a/robots/z1_description/xacro/const.xacro b/robots/z1_description/xacro/const.xacro
index 4317289..e22cd58 100644
--- a/robots/z1_description/xacro/const.xacro
+++ b/robots/z1_description/xacro/const.xacro
@@ -114,23 +114,21 @@
-
-
-
+
-
-
+
+
-
-
-
-
+
+
+
+
diff --git a/robots/z1_description/xacro/gazebo.xacro b/robots/z1_description/xacro/gazebo.xacro
index 4db3f34..acf0b7e 100644
--- a/robots/z1_description/xacro/gazebo.xacro
+++ b/robots/z1_description/xacro/gazebo.xacro
@@ -1,6 +1,5 @@
-
/z1_gazebo
@@ -19,6 +18,4 @@
true
-
-
diff --git a/robots/z1_description/xacro/robot.xacro b/robots/z1_description/xacro/robot.xacro
index 49f3ad6..7f57a05 100644
--- a/robots/z1_description/xacro/robot.xacro
+++ b/robots/z1_description/xacro/robot.xacro
@@ -55,12 +55,6 @@
-
@@ -141,7 +135,7 @@
-
+
@@ -171,9 +165,9 @@
-
+
-
+
diff --git a/robots/z1_description/xacro/z1.urdf b/robots/z1_description/xacro/z1.urdf
index c46e19e..a0d2b36 100644
--- a/robots/z1_description/xacro/z1.urdf
+++ b/robots/z1_description/xacro/z1.urdf
@@ -4,7 +4,6 @@
-
/z1_gazebo
@@ -30,7 +29,7 @@
-
+
@@ -57,15 +56,9 @@
-
+
-
@@ -78,13 +71,13 @@
-
+
-
+
@@ -117,13 +110,13 @@
-
+
-
+
@@ -136,7 +129,7 @@
-
+
@@ -150,20 +143,20 @@
-
+
-
+
-
+
-
+
@@ -177,13 +170,13 @@
-
+
-
+
@@ -204,7 +197,7 @@
-
+
diff --git a/z1_controller/CMakeLists.txt b/z1_controller/CMakeLists.txt
deleted file mode 100644
index 875d168..0000000
--- a/z1_controller/CMakeLists.txt
+++ /dev/null
@@ -1,42 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(z1_controller)
-
-find_package(gazebo REQUIRED)
-find_package(catkin REQUIRED COMPONENTS
- controller_manager
- std_msgs
- joint_state_controller
- robot_state_publisher
- roscpp
- gazebo_ros
- geometry_msgs
- unitree_legged_msgs
- tf
-)
-
-catkin_package(
- CATKIN_DEPENDS
- unitree_legged_msgs
-)
-
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${GAZEBO_INCLUDE_DIRS}
-)
-
-link_directories(${GAZEBO_LIBRARY_DIRS})
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-
-add_library(${PROJECT_NAME}
- src/unitreeArm.cpp
-)
-
-add_dependencies(${PROJECT_NAME} unitree_legged_msgs_gencpp)
-
-target_link_libraries(${PROJECT_NAME}
- ${catkin_LIBRARIES} ${EXTRA_LIBS}
-)
-
-add_executable(unitree_move_z1 src/basic.cpp)
-target_link_libraries(unitree_move_z1 ${PROJECT_NAME} ${catkin_LIBRARIES})
\ No newline at end of file
diff --git a/z1_controller/include/unitreeArm.h b/z1_controller/include/unitreeArm.h
deleted file mode 100644
index ec4d997..0000000
--- a/z1_controller/include/unitreeArm.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#ifndef __UNITREEARM_H
-#define __UNITREEARM_H
-
-#include
-#include "unitree_legged_msgs/MotorCmd.h"
-#include "unitree_legged_msgs/MotorState.h"
-
-namespace unitreeArm {
-
-class IOROS
-{
-public:
- IOROS();
- ~IOROS();
-
- void sendCmd(double* targetPos, double duration);
-
-private:
- void _joint01Callback(const unitree_legged_msgs::MotorState& msg);
- void _joint02Callback(const unitree_legged_msgs::MotorState& msg);
- void _joint03Callback(const unitree_legged_msgs::MotorState& msg);
- void _joint04Callback(const unitree_legged_msgs::MotorState& msg);
- void _joint05Callback(const unitree_legged_msgs::MotorState& msg);
- void _joint06Callback(const unitree_legged_msgs::MotorState& msg);
- void _gripperCallback(const unitree_legged_msgs::MotorState& msg);
-
- ros::NodeHandle _nm;
- ros::Publisher _servo_pub[7];
- ros::Subscriber _servo_sub[7];
- unitree_legged_msgs::MotorCmd _joint_cmd[7];
- unitree_legged_msgs::MotorState _joint_state[7];
-
-};
-
-}
-
-#endif
diff --git a/z1_controller/package.xml b/z1_controller/package.xml
deleted file mode 100644
index 151c29f..0000000
--- a/z1_controller/package.xml
+++ /dev/null
@@ -1,34 +0,0 @@
-
-
- z1_controller
- 0.0.0
- The z1_controller package
-
- unitree
- TODO
-
- catkin
-
- catkin
- genmsg
- controller_manager
- joint_state_controller
- robot_state_publisher
- roscpp
- std_msgs
- controller_manager
- joint_state_controller
- robot_state_publisher
- roscpp
- std_msgs
- controller_manager
- joint_state_controller
- robot_state_publisher
- roscpp
- std_msgs
- unitree_legged_msgs
-
-
-
-
-
diff --git a/z1_controller/src/basic.cpp b/z1_controller/src/basic.cpp
deleted file mode 100644
index 3b5c0e3..0000000
--- a/z1_controller/src/basic.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "unitreeArm.h"
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "z1_controller");
- ros::AsyncSpinner subSpinner(1); // one threads
- subSpinner.start();
- usleep(300000); // must wait 300ms, to get first state
-
- unitreeArm::IOROS ioInter;
-
- // move z1 to [forward]
- double forward[7] = {0, 1.5, -1, -0.54, 0, 0, -1};
- ioInter.sendCmd(forward, 1000);
-
- return 0;
-}
\ No newline at end of file
diff --git a/z1_controller/src/unitreeArm.cpp b/z1_controller/src/unitreeArm.cpp
deleted file mode 100644
index 6cf2539..0000000
--- a/z1_controller/src/unitreeArm.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-#include "unitreeArm.h"
-
-namespace unitreeArm {
-
-IOROS::IOROS()
-{
- // publisher init
- _servo_pub[0] = _nm.advertise("/z1_gazebo/Joint01_controller/command", 1);
- _servo_pub[1] = _nm.advertise("/z1_gazebo/Joint02_controller/command", 1);
- _servo_pub[2] = _nm.advertise("/z1_gazebo/Joint03_controller/command", 1);
- _servo_pub[3] = _nm.advertise("/z1_gazebo/Joint04_controller/command", 1);
- _servo_pub[4] = _nm.advertise("/z1_gazebo/Joint05_controller/command", 1);
- _servo_pub[5] = _nm.advertise("/z1_gazebo/Joint06_controller/command", 1);
- _servo_pub[6] = _nm.advertise("/z1_gazebo/gripper_controller/command", 1);
-
- // subscriber init
- _servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint01Callback, this);
- _servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint02Callback, this);
- _servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint03Callback, this);
- _servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint04Callback, this);
- _servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint05Callback, this);
- _servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint06Callback, this);
- _servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
-
- // parameter init
- for(size_t i=0; i<7; ++i)
- {
- _joint_cmd[i].mode = 10;
- _joint_cmd[i].Kp = 300.0;
- _joint_cmd[i].Kd = 5;
- _joint_cmd[i].q = 0;
- _joint_cmd[i].dq = 0;
- _joint_cmd[i].tau = 0;
- }
-}
-
-IOROS::~IOROS()
-{
-
-}
-
-void IOROS::_joint01Callback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[0].mode = msg.mode;
- _joint_state[0].q = msg.q;
- _joint_state[0].dq = msg.dq;
- _joint_state[0].ddq = msg.ddq;
- _joint_state[0].tauEst = msg.tauEst;
-}
-
-void IOROS::_joint02Callback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[1].mode = msg.mode;
- _joint_state[1].q = msg.q;
- _joint_state[1].dq = msg.dq;
- _joint_state[1].ddq = msg.ddq;
- _joint_state[1].tauEst = msg.tauEst;
-}
-
-void IOROS::_joint03Callback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[2].mode = msg.mode;
- _joint_state[2].q = msg.q;
- _joint_state[2].dq = msg.dq;
- _joint_state[2].ddq = msg.ddq;
- _joint_state[2].tauEst = msg.tauEst;
-}
-
-void IOROS::_joint04Callback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[3].mode = msg.mode;
- _joint_state[3].q = msg.q;
- _joint_state[3].dq = msg.dq;
- _joint_state[3].ddq = msg.ddq;
- _joint_state[3].tauEst = msg.tauEst;
-}
-
-void IOROS::_joint05Callback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[4].mode = msg.mode;
- _joint_state[4].q = msg.q;
- _joint_state[4].dq = msg.dq;
- _joint_state[4].ddq = msg.ddq;
- _joint_state[4].tauEst = msg.tauEst;
-}
-
-void IOROS::_joint06Callback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[5].mode = msg.mode;
- _joint_state[5].q = msg.q;
- _joint_state[5].dq = msg.dq;
- _joint_state[5].ddq = msg.ddq;
- _joint_state[5].tauEst = msg.tauEst;
-}
-
-void IOROS::_gripperCallback(const unitree_legged_msgs::MotorState& msg)
-{
- _joint_state[6].mode = msg.mode;
- _joint_state[6].q = msg.q;
- _joint_state[6].dq = msg.dq;
- _joint_state[6].ddq = msg.ddq;
- _joint_state[6].tauEst = msg.tauEst;
-}
-
-void IOROS::sendCmd(double* targetPos, double duration)
-{
- double pos[7], lastPos[7], percent;
- for(size_t i=0; i<7; ++i) lastPos[i] = _joint_state[i].q;
- for(int i=1; i<=duration; i++)
- {
- if(!ros::ok()) break;
- percent = (double)i/duration;
- for(size_t j=0; j<7; ++j)
- {
- _joint_cmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent;
- _servo_pub[j].publish(_joint_cmd[j]);
- }
- ros::spinOnce();
- usleep(1000);
- }
-}
-
-}
\ No newline at end of file