EmbodiedAI
/
unitree_ros
mirror of https://github.com/unitreerobotics/unitree_ros.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

modify z1_urdf

This commit is contained in:
Agnel Wang 2023-06-27 16:23:00 +08:00
parent 2e566b78bc
commit fa9b011d74
11 changed files with 28 additions and 303 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

@ -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. 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) see [z1_documentation](https://dev-z1.unitree.com)
You can also send ros commands directly to z1 robot:
```
rosrun z1_controller unitree_move_z1
```

6
robots/aliengoZ1_description/xacro/robot.xacro
View File

@ -219,7 +219,7 @@
<geometry> <geometry>
<cylinder length="0.059" radius="${motor_diameter}"/> <cylinder length="0.059" radius="${motor_diameter}"/>
</geometry> </geometry>
<origin rpy="0 ${PI/2.0} 0" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/> <origin rpy="0 ${PI/2.0} ${PI/2.0}" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/> <origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/>
@ -249,9 +249,9 @@
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<cylinder length="0.073" radius="${motor_diameter}"/> <cylinder length="${motor_height + 2*0.008}" radius="${motor_diameter}"/>
</geometry> </geometry>
<origin rpy="${PI/2.0} 0 0" xyz="0.0472 0 0"/> <origin rpy="0 0 0" xyz="0.072 0 0"/>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/> <origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/>

16
robots/z1_description/xacro/const.xacro
View File

@ -114,23 +114,21 @@
<xacro:property name="GripperMover_Izz" value="0.00035728"/> <xacro:property name="GripperMover_Izz" value="0.00035728"/>
<!-- joint limits --> <!-- joint limits -->
<!-- <xacro:property name="jointDamping" value="3.0"/> -->
<!-- <xacro:property name="jointFriction" value="2.0"/> -->
<xacro:property name="jointDamping" value="1.0"/> <xacro:property name="jointDamping" value="1.0"/>
<xacro:property name="jointFriction" value="1.0"/> <xacro:property name="jointFriction" value="1.0"/>
<xacro:property name="torqueMax" value="30.0"/> <xacro:property name="torqueMax" value="30.0"/>
<xacro:property name="velocityMax" value="10"/> <xacro:property name="velocityMax" value="3.1415"/>
<xacro:property name="joint1_PositionMin" value="${-PI*150/180}"/> <xacro:property name="joint1_PositionMin" value="${-PI*150/180}"/>
<xacro:property name="joint1_PositionMax" value="${PI*150/180}"/> <xacro:property name="joint1_PositionMax" value="${PI*150/180}"/>
<xacro:property name="joint2_PositionMin" value="0.0"/> <xacro:property name="joint2_PositionMin" value="0.0"/>
<xacro:property name="joint2_PositionMax" value="${PI}"/> <xacro:property name="joint2_PositionMax" value="${PI*170/180}"/>
<xacro:property name="joint3_PositionMin" value="${-PI*274/180}"/> <xacro:property name="joint3_PositionMin" value="${-PI*165/180}"/>
<xacro:property name="joint3_PositionMax" value="0.0"/> <xacro:property name="joint3_PositionMax" value="0.0"/>
<xacro:property name="joint4_PositionMin" value="${-PI*100/180}"/> <xacro:property name="joint4_PositionMin" value="${-PI*87/180}"/>
<xacro:property name="joint4_PositionMax" value="${PI*90/180}"/> <xacro:property name="joint4_PositionMax" value="${PI*87/180}"/>
<xacro:property name="joint5_PositionMin" value="${-PI*99/180}"/> <xacro:property name="joint5_PositionMin" value="${-PI*77/180}"/>
<xacro:property name="joint5_PositionMax" value="${PI*99/180}"/> <xacro:property name="joint5_PositionMax" value="${PI*77/180}"/>
<xacro:property name="joint6_PositionMin" value="${-PI*160/180}"/> <xacro:property name="joint6_PositionMin" value="${-PI*160/180}"/>
<xacro:property name="joint6_PositionMax" value="${PI*160/180}"/> <xacro:property name="joint6_PositionMax" value="${PI*160/180}"/>
<xacro:property name="Gripper_PositionMin" value="${-PI*90/180}"/> <xacro:property name="Gripper_PositionMin" value="${-PI*90/180}"/>

3
robots/z1_description/xacro/gazebo.xacro
View File

@ -1,6 +1,5 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot> <robot>
<!-- ros_control plugin -->
<gazebo> <gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/z1_gazebo</robotNamespace> <robotNamespace>/z1_gazebo</robotNamespace>
@ -19,6 +18,4 @@
<gazebo reference="link06"> <gazebo reference="link06">
<self_collide>true</self_collide> <self_collide>true</self_collide>
</gazebo> </gazebo>
</robot> </robot>

12
robots/z1_description/xacro/robot.xacro
View File

@ -55,12 +55,6 @@
<mesh filename="package://z1_description/meshes/visual/z1_Link01.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link01.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<!-- <collision>
<geometry>
<cylinder length="${2*motor_height}" radius="${motor_diameter}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision> -->
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${L01_ComX} ${L01_ComY} ${L01_ComZ}"/> <origin rpy="0 0 0" xyz="${L01_ComX} ${L01_ComY} ${L01_ComZ}"/>
<mass value="${L01_Mass}"/> <mass value="${L01_Mass}"/>
@ -141,7 +135,7 @@
<geometry> <geometry>
<cylinder length="0.059" radius="${motor_diameter}"/> <cylinder length="0.059" radius="${motor_diameter}"/>
</geometry> </geometry>
<origin rpy="0 ${PI/2.0} 0" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/> <origin rpy="0 ${PI/2.0} ${PI/2.0}" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/> <origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/>
@ -171,9 +165,9 @@
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<cylinder length="0.073" radius="${motor_diameter}"/> <cylinder length="${motor_height + 2*0.008}" radius="${motor_diameter}"/>
</geometry> </geometry>
<origin rpy="${PI/2.0} 0 0" xyz="0.0472 0 0"/> <origin rpy="0 0 0" xyz="0.072 0 0"/>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/> <origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/>

35
robots/z1_description/xacro/z1.urdf
View File

@ -4,7 +4,6 @@
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --> <!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== --> <!-- =================================================================================== -->
<robot name="z1_description"> <robot name="z1_description">
<!-- ros_control plugin -->
<gazebo> <gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control"> <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/z1_gazebo</robotNamespace> <robotNamespace>/z1_gazebo</robotNamespace>
@ -30,7 +29,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link00.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link00.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
@ -57,15 +56,9 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link01.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link01.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<!-- <collision>
<geometry>
<cylinder length="${2*motor_height}" radius="${motor_diameter}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision> -->
<inertial> <inertial>
<origin rpy="0 0 0" xyz="2.47e-06 -0.00025198 0.02317169"/> <origin rpy="0 0 0" xyz="2.47e-06 -0.00025198 0.02317169"/>
<mass value="0.67332551"/> <mass value="0.67332551"/>
@ -78,13 +71,13 @@
<child link="link02"/> <child link="link02"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="2.0" friction="2.0"/> <dynamics damping="2.0" friction="2.0"/>
<limit effort="60.0" lower="0.0" upper="3.141592653589793" velocity="10"/> <limit effort="60.0" lower="0.0" upper="2.9670597283903604" velocity="10"/>
</joint> </joint>
<link name="link02"> <link name="link02">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link02.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link02.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
@ -117,13 +110,13 @@
<child link="link03"/> <child link="link03"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="1.0" friction="1.0"/> <dynamics damping="1.0" friction="1.0"/>
<limit effort="30.0" lower="-4.782202150464463" upper="0.0" velocity="10"/> <limit effort="30.0" lower="-2.8797932657906435" upper="0.0" velocity="10"/>
</joint> </joint>
<link name="link03"> <link name="link03">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link03.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link03.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
@ -136,7 +129,7 @@
<geometry> <geometry>
<cylinder length="0.059" radius="0.0325"/> <cylinder length="0.059" radius="0.0325"/>
</geometry> </geometry>
<origin rpy="0 1.5707963267948966 0" xyz="0.2205 0 0.055"/> <origin rpy="0 1.5707963267948966 1.5707963267948966" xyz="0.2205 0 0.055"/>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0.10609208 -0.00541815 0.03476383"/> <origin rpy="0 0 0" xyz="0.10609208 -0.00541815 0.03476383"/>
@ -150,20 +143,20 @@
<child link="link04"/> <child link="link04"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<dynamics damping="1.0" friction="1.0"/> <dynamics damping="1.0" friction="1.0"/>
<limit effort="30.0" lower="-1.7453292519943295" upper="1.5707963267948966" velocity="10"/> <limit effort="30.0" lower="-1.5184364492350666" upper="1.5184364492350666" velocity="10"/>
</joint> </joint>
<link name="link04"> <link name="link04">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link04.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link04.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<cylinder length="0.073" radius="0.0325"/> <cylinder length="0.067" radius="0.0325"/>
</geometry> </geometry>
<origin rpy="1.5707963267948966 0 0" xyz="0.0472 0 0"/> <origin rpy="0 0 0" xyz="0.072 0 0"/>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0.04366681 0.00364738 -0.00170192"/> <origin rpy="0 0 0" xyz="0.04366681 0.00364738 -0.00170192"/>
@ -177,13 +170,13 @@
<child link="link05"/> <child link="link05"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<dynamics damping="1.0" friction="1.0"/> <dynamics damping="1.0" friction="1.0"/>
<limit effort="30.0" lower="-1.7278759594743864" upper="1.7278759594743864" velocity="10"/> <limit effort="30.0" lower="-1.3439035240356338" upper="1.3439035240356338" velocity="10"/>
</joint> </joint>
<link name="link05"> <link name="link05">
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link05.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link05.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
@ -204,7 +197,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://z1_description/meshes/z1_Link06.dae" scale="1 1 1"/> <mesh filename="package://z1_description/meshes/visual/z1_Link06.dae" scale="1 1 1"/>
</geometry> </geometry>
</visual> </visual>
<collision> <collision>

42
z1_controller/CMakeLists.txt
View File

@ -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})

37
z1_controller/include/unitreeArm.h
View File

@ -1,37 +0,0 @@
#ifndef __UNITREEARM_H
#define __UNITREEARM_H
#include <ros/ros.h>
#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

34
z1_controller/package.xml
View File

@ -1,34 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>z1_controller</name>
<version>0.0.0</version>
<description>The z1_controller package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>genmsg</buildtool_depend>
<build_depend>controller_manager</build_depend>
<build_depend>joint_state_controller</build_depend>
<build_depend>robot_state_publisher</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>controller_manager</build_export_depend>
<build_export_depend>joint_state_controller</build_export_depend>
<build_export_depend>robot_state_publisher</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>controller_manager</exec_depend>
<exec_depend>joint_state_controller</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<depend>unitree_legged_msgs</depend>
<export>
</export>
</package>

17
z1_controller/src/basic.cpp
View File

@ -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;
}

123
z1_controller/src/unitreeArm.cpp
View File

@ -1,123 +0,0 @@
#include "unitreeArm.h"
namespace unitreeArm {
IOROS::IOROS()
{
// publisher init
_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);
// 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);
}
}
}