EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat: [Destructive Update]del submodules unitree_ros

This commit is contained in:
fan-ziqi 2024-05-23 20:55:35 +08:00
parent 24e8018e0d
commit 382702c035
40 changed files with 3401 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -1,3 +0,0 @@
[submodule "src/unitree_ros"]
path = src/unitree_ros
url = https://github.com/unitreerobotics/unitree_ros.git

21
README.md
View File

@ -6,17 +6,10 @@ Simulation verification and physical deployment of the quadruped robot's reinfor
## Preparation ## Preparation
Clone the code (sync submodules) Clone the code
```bash ```bash
git clone --recursive https://github.com/fan-ziqi/rl_sar.git git clone https://github.com/fan-ziqi/rl_sar.git
```
If there are updates:
```bash
git pull
git submodule update --remote --recursive
``` ```
## Dependency ## Dependency
@ -182,12 +175,10 @@ In the following, let ROBOT represent the name of your robot.
4. Add a new launch file in the rl_sar/launch folder. Refer to other launch files for guidance on modification. 4. Add a new launch file in the rl_sar/launch folder. Refer to other launch files for guidance on modification.
5. Change ROBOT_NAME to ROBOT in rl_xxx.cpp. 5. Change ROBOT_NAME to ROBOT in rl_xxx.cpp.
6. Compile and run. 6. Compile and run.
7. If the torque of your robot's joints exceeds 50Nm, you need to modify line 180 in `rl_sar/src/unitree_ros/unitree_legged_control/src/joint_controller.cpp` to:
```cpp ## Reference
// calcTorque = computeTorque(currentPos, currentVel, servoCmd);
calcTorque = servoCmd.posStiffness * (servoCmd.pos - currentPos) + servoCmd.velStiffness * (servoCmd.vel - currentVel) + servoCmd.torque; [unitree_ros](https://github.com/unitreerobotics/unitree_ros)
```
This will remove the 50Nm limit.
## Citation ## Citation

22
README_CN.md
View File

@ -6,17 +6,10 @@
## 准备 ## 准备
拉取代码（同步拉取子模块） 拉取代码
```bash ```bash
git clone --recursive https://github.com/fan-ziqi/rl_sar.git git clone https://github.com/fan-ziqi/rl_sar.git
```
如有更新：
```bash
git pull
git submodule update --remote --recursive
``` ```
## 依赖 ## 依赖
@ -172,7 +165,6 @@ rosrun rl_sar rl_real_a1
bash kill_cyberdog.sh bash kill_cyberdog.sh
``` ```
## 添加你的机器人 ## 添加你的机器人
下文中将ROBOT代表机器人名称 下文中将ROBOT代表机器人名称
@ -183,12 +175,10 @@ rosrun rl_sar rl_real_a1
4. 在rl_sar/launch文件夹中添加一个新的launch文件，请参考其他launch文件自行修改 4. 在rl_sar/launch文件夹中添加一个新的launch文件，请参考其他launch文件自行修改
5. 修改rl_xxx.cpp中的ROBOT_NAME为ROBOT 5. 修改rl_xxx.cpp中的ROBOT_NAME为ROBOT
6. 编译运行 6. 编译运行
7. 若您的机器人关节力矩大于50Nm，则需要修改`rl_sar/src/unitree_ros/unitree_legged_control/src/joint_controller.cpp`中180行为：
```cpp ## 参考
// calcTorque = computeTorque(currentPos, currentVel, servoCmd);
calcTorque = servoCmd.posStiffness * (servoCmd.pos - currentPos) + servoCmd.velStiffness * (servoCmd.vel - currentVel) + servoCmd.torque; [unitree_ros](https://github.com/unitreerobotics/unitree_ros)
```
这样会解除50Nm的限制
## 引用 ## 引用

5
src/rl_sar/CMakeLists.txt
View File

@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
tf tf
geometry_msgs geometry_msgs
robot_msgs robot_msgs
unitree_legged_msgs robot_joint_controller
) )
find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
@ -36,7 +36,7 @@ include_directories(${YAML_CPP_INCLUDE_DIR})
catkin_package( catkin_package(
CATKIN_DEPENDS CATKIN_DEPENDS
unitree_legged_msgs robot_joint_controller
) )
include_directories(library/unitree_legged_sdk_3.2/include) include_directories(library/unitree_legged_sdk_3.2/include)
@ -50,7 +50,6 @@ include_directories(
include include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${unitree_legged_sdk_INCLUDE_DIRS} ${unitree_legged_sdk_INCLUDE_DIRS}
../unitree_controller/include
library/matplotlibcpp library/matplotlibcpp
library/observation_buffer library/observation_buffer
library/rl_sdk library/rl_sdk

5
src/rl_sar/include/rl_real_a1.hpp
View File

@ -1,13 +1,8 @@
#ifndef RL_REAL_HPP #ifndef RL_REAL_HPP
#define RL_REAL_HPP #define RL_REAL_HPP
// #include "../library/rl_sdk/rl_sdk.hpp"
#include "rl_sdk.hpp" #include "rl_sdk.hpp"
#include "observation_buffer.hpp" #include "observation_buffer.hpp"
#include <unitree_legged_msgs/LowCmd.h>
#include "unitree_legged_msgs/LowState.h"
#include <unitree_legged_msgs/MotorCmd.h>
#include <unitree_legged_msgs/MotorState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h" #include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "unitree_legged_sdk/unitree_joystick.h" #include "unitree_legged_sdk/unitree_joystick.h"
#include <csignal> #include <csignal>

6
src/rl_sar/include/rl_sim.hpp
View File

@ -7,11 +7,10 @@
#include <gazebo_msgs/ModelStates.h> #include <gazebo_msgs/ModelStates.h>
#include <sensor_msgs/JointState.h> #include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include "unitree_legged_msgs/MotorCmd.h"
#include "unitree_legged_sdk/loop.h" #include "unitree_legged_sdk/loop.h"
#include <csignal> #include <csignal>
// #include "robot_msgs/MotorCommand.h" #include "robot_msgs/MotorCommand.h"
// #include "robot_msgs/RobotState.h" // #include "robot_msgs/RobotState.h"
// #include "robot_msgs/RobotCommand.h" // #include "robot_msgs/RobotCommand.h"
@ -53,9 +52,8 @@ private:
ros::Subscriber cmd_vel_subscriber_; ros::Subscriber cmd_vel_subscriber_;
std::map<std::string, ros::Publisher> torque_publishers; std::map<std::string, ros::Publisher> torque_publishers;
std::vector<unitree_legged_msgs::MotorCmd> motor_commands; std::vector<robot_msgs::MotorCommand> motor_commands;
// std::vector<robot_msgs::MotorCommand> motor_commands;
// robot_msgs::RobotState robot_state; // robot_msgs::RobotState robot_state;
// robot_msgs::RobotCommand robot_command; // robot_msgs::RobotCommand robot_command;

7
src/rl_sar/launch/start_a1.launch
View File

@ -51,11 +51,4 @@
<remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/> <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
</node> </node>
<!-- <node pkg="unitree_gazebo" type="servo" name="servo" required="true" output="screen"/> -->
<!-- load the parameter unitree_controller -->
<include file="$(find unitree_controller)/launch/set_ctrl.launch">
<arg name="rname" value="$(arg rname)"/>
</include>
</launch> </launch>

7
src/rl_sar/launch/start_cyberdog.launch
View File

@ -49,11 +49,4 @@
<remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/> <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
</node> </node>
<!-- <node pkg="unitree_gazebo" type="servo" name="servo" required="true" output="screen"/> -->
<!-- load the parameter unitree_controller -->
<include file="$(find unitree_controller)/launch/set_ctrl.launch">
<arg name="rname" value="$(arg rname)"/>
</include>
</launch> </launch>

2
src/rl_sar/package.xml
View File

@ -26,7 +26,7 @@
<exec_depend>roscpp</exec_depend> <exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
<depend>robot_msgs</depend> <depend>robot_msgs</depend>
<depend>unitree_legged_msgs</depend> <depend>robot_joint_controller</depend>
<export> <export>

7
src/rl_sar/src/rl_sim.cpp
View File

@ -54,7 +54,7 @@ RL_Sim::RL_Sim()
for (int i = 0; i < params.num_of_dofs; ++i) for (int i = 0; i < params.num_of_dofs; ++i)
{ {
torque_publishers[params.joint_names[i]] = nh.advertise<unitree_legged_msgs::MotorCmd>( torque_publishers[params.joint_names[i]] = nh.advertise<robot_msgs::MotorCommand>(
ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10); ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10);
} }
@ -94,13 +94,12 @@ void RL_Sim::RobotControl()
motiontime++; motiontime++;
for (int i = 0; i < params.num_of_dofs; ++i) for (int i = 0; i < params.num_of_dofs; ++i)
{ {
motor_commands[i].mode = 0x0A;
motor_commands[i].q = output_dof_pos[0][i].item<double>(); motor_commands[i].q = output_dof_pos[0][i].item<double>();
motor_commands[i].dq = 0; motor_commands[i].dq = 0;
// motor_commands[i].Kp = params.stiffness; // motor_commands[i].Kp = params.stiffness;
// motor_commands[i].Kd = params.damping; // motor_commands[i].Kd = params.damping;
motor_commands[i].Kp = params.p_gains[0][i].item<double>(); motor_commands[i].kp = params.p_gains[0][i].item<double>();
motor_commands[i].Kd = params.d_gains[0][i].item<double>(); motor_commands[i].kd = params.d_gains[0][i].item<double>();
// motor_commands[i].tau = output_torques[0][i].item<double>(); // motor_commands[i].tau = output_torques[0][i].item<double>();
motor_commands[i].tau = 0; motor_commands[i].tau = 0;

31
src/robot_joint_controller/CMakeLists.txt Normal file
View File

@ -0,0 +1,31 @@
cmake_minimum_required(VERSION 2.8.3)
project(robot_joint_controller)
find_package(catkin REQUIRED COMPONENTS
controller_interface
hardware_interface
pluginlib
roscpp
realtime_tools
robot_msgs
)
catkin_package(
CATKIN_DEPENDS
robot_msgs
controller_interface
hardware_interface
pluginlib
roscpp
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR})
link_directories(${catkin_LIB_DIRS} lib)
add_library(robot_joint_controller
src/robot_joint_controller.cpp
)
target_link_libraries(robot_joint_controller ${catkin_LIBRARIES})

75
src/robot_joint_controller/include/robot_joint_controller.h Normal file
View File

@ -0,0 +1,75 @@
#ifndef ROBOT_JOINT_CONTROLLER_H
#define ROBOT_JOINT_CONTROLLER_H
#include <ros/node_handle.h>
#include <urdf/model.h>
#include <control_toolbox/pid.h>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/condition.hpp>
#include <realtime_tools/realtime_publisher.h>
#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <std_msgs/Float64.h>
#include <realtime_tools/realtime_buffer.h>
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include "robot_msgs/MotorCommand.h"
#include "robot_msgs/MotorState.h"
#include <geometry_msgs/WrenchStamped.h>
#include <stdio.h>
#include <stdint.h>
#include <algorithm>
#include <math.h>
#define PosStopF (2.146E+9f)
#define VelStopF (16000.0f)
typedef struct
{
uint8_t mode;
double pos;
double posStiffness;
double vel;
double velStiffness;
double torque;
} ServoCommand;
namespace robot_joint_controller
{
class RobotJointController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
private:
hardware_interface::JointHandle joint;
ros::Subscriber sub_command, sub_ft;
control_toolbox::Pid pid_controller_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_msgs::MotorState> > controller_state_publisher_ ;
public:
std::string name_space;
std::string joint_name;
urdf::JointConstSharedPtr joint_urdf;
realtime_tools::RealtimeBuffer<robot_msgs::MotorCommand> command;
robot_msgs::MotorCommand lastCommand;
robot_msgs::MotorState lastState;
ServoCommand servoCommand;
RobotJointController();
~RobotJointController();
virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
virtual void starting(const ros::Time& time);
virtual void update(const ros::Time& time, const ros::Duration& period);
virtual void stopping();
void setCommandCB(const robot_msgs::MotorCommandConstPtr& msg);
void positionLimits(double &position);
void velocityLimits(double &velocity);
void effortLimits(double &effort);
void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
};
}
#endif

27
src/robot_joint_controller/package.xml Normal file
View File

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<package format="2">
<name>robot_joint_controller</name>
<version>0.0.0</version>
<description>The robot_joint_controller package</description>
<maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
<license>Apache</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>controller_interface</build_depend>
<build_depend>hardware_interface</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>controller_interface</build_export_depend>
<build_export_depend>hardware_interface</build_export_depend>
<build_export_depend>pluginlib</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>controller_interface</exec_depend>
<exec_depend>hardware_interface</exec_depend>
<exec_depend>pluginlib</exec_depend>
<exec_depend>roscpp</exec_depend>
<depend>robot_msgs</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<controller_interface plugin="${prefix}/robot_joint_controller_plugins.xml"/>
</export>
</package>

8
src/robot_joint_controller/robot_joint_controller_plugins.xml Normal file
View File

@ -0,0 +1,8 @@
<library path="lib/librobot_joint_controller">
<class name="robot_joint_controller/RobotJointController"
type="robot_joint_controller::RobotJointController"
base_class_type="controller_interface::ControllerBase"/>
<description>
The robot joint controller.
</description>
</library>

186
src/robot_joint_controller/src/robot_joint_controller.cpp Normal file
View File

@ -0,0 +1,186 @@
#include "robot_joint_controller.h"
#include <pluginlib/class_list_macros.h>
// #define rqtTune // use rqt or not
double clamp(double& value, double min, double max) {
if (value < min) {
value = min;
} else if (value > max) {
value = max;
}
return value;
}
namespace robot_joint_controller
{
RobotJointController::RobotJointController(){
memset(&lastCommand, 0, sizeof(robot_msgs::MotorCommand));
memset(&lastState, 0, sizeof(robot_msgs::MotorState));
memset(&servoCommand, 0, sizeof(ServoCommand));
}
RobotJointController::~RobotJointController(){
sub_ft.shutdown();
sub_command.shutdown();
}
void RobotJointController::setCommandCB(const robot_msgs::MotorCommandConstPtr& msg)
{
lastCommand.q = msg->q;
lastCommand.kp = msg->kp;
lastCommand.dq = msg->dq;
lastCommand.kd = msg->kd;
lastCommand.tau = msg->tau;
// the writeFromNonRT can be used in RT, if you have the guarantee that
// * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
// * there is only one single rt thread
command.writeFromNonRT(lastCommand);
}
// Controller initialization in non-realtime
bool RobotJointController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
name_space = n.getNamespace();
if (!n.getParam("joint", joint_name)){
ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str());
return false;
}
// load pid param from ymal only if rqt need
#ifdef rqtTune
// Load PID Controller using gains set on parameter server
if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
return false;
#endif
urdf::Model urdf; // Get URDF info about joint
if (!urdf.initParamWithNodeHandle("robot_description", n)){
ROS_ERROR("Failed to parse urdf file");
return false;
}
joint_urdf = urdf.getJoint(joint_name);
if (!joint_urdf){
ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
return false;
}
joint = robot->getHandle(joint_name);
// Start command subscriber
sub_command = n.subscribe("command", 20, &RobotJointController::setCommandCB, this);
// Start realtime state publisher
controller_state_publisher_.reset(
new realtime_tools::RealtimePublisher<robot_msgs::MotorState>(n, name_space + "/state", 1));
return true;
}
void RobotJointController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
{
pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
}
void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
{
pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
}
void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
bool dummy;
pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
}
// Controller startup in realtime
void RobotJointController::starting(const ros::Time& time)
{
double init_pos = joint.getPosition();
lastCommand.q = init_pos;
lastState.q = init_pos;
lastCommand.dq = 0;
lastState.dq = 0;
lastCommand.tau = 0;
lastState.tauEst = 0;
command.initRT(lastCommand);
pid_controller_.reset();
}
// Controller update loop in realtime
void RobotJointController::update(const ros::Time& time, const ros::Duration& period)
{
double currentPos, currentVel, calcTorque;
lastCommand = *(command.readFromRT());
// set command data
servoCommand.pos = lastCommand.q;
positionLimits(servoCommand.pos);
servoCommand.posStiffness = lastCommand.kp;
if(fabs(lastCommand.q - PosStopF) < 0.00001){
servoCommand.posStiffness = 0;
}
servoCommand.vel = lastCommand.dq;
velocityLimits(servoCommand.vel);
servoCommand.velStiffness = lastCommand.kd;
if(fabs(lastCommand.dq - VelStopF) < 0.00001){
servoCommand.velStiffness = 0;
}
servoCommand.torque = lastCommand.tau;
effortLimits(servoCommand.torque);
// rqt set P D gains
#ifdef rqtTune
double i, i_max, i_min;
getGains(servoCommand.posStiffness,i,servoCommand.velStiffness,i_max,i_min);
#endif
currentPos = joint.getPosition();
// currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec());
// calcTorque = computeTorque(currentPos, currentVel, servoCommand);
currentVel = (currentPos - (double)lastState.q) / period.toSec();
calcTorque = servoCommand.posStiffness * (servoCommand.pos - currentPos) + servoCommand.velStiffness * (servoCommand.vel - currentVel) + servoCommand.torque;
effortLimits(calcTorque);
joint.setCommand(calcTorque);
lastState.q = currentPos;
lastState.dq = currentVel;
// lastState.tauEst = calcTorque;
lastState.tauEst = joint.getEffort();
// publish state
if (controller_state_publisher_ && controller_state_publisher_->trylock()) {
controller_state_publisher_->msg_.q = lastState.q;
controller_state_publisher_->msg_.dq = lastState.dq;
controller_state_publisher_->msg_.tauEst = lastState.tauEst;
controller_state_publisher_->unlockAndPublish();
}
}
// Controller stopping in realtime
void RobotJointController::stopping(){}
void RobotJointController::positionLimits(double &position)
{
if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
clamp(position, joint_urdf->limits->lower, joint_urdf->limits->upper);
}
void RobotJointController::velocityLimits(double &velocity)
{
if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
clamp(velocity, -joint_urdf->limits->velocity, joint_urdf->limits->velocity);
}
void RobotJointController::effortLimits(double &effort)
{
if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
clamp(effort, -joint_urdf->limits->effort, joint_urdf->limits->effort);
}
} // namespace
// Register controller to pluginlib
PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerBase);

19
src/robots/a1_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,19 @@
cmake_minimum_required(VERSION 2.8.3)
project(a1_description)
find_package(catkin REQUIRED COMPONENTS
genmsg
roscpp
std_msgs
tf
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
# include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)

70
src/robots/a1_description/config/robot_control.yaml Normal file
View File

@ -0,0 +1,70 @@
a1_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

23
src/robots/a1_description/launch/a1_rviz.launch Normal file
View File

@ -0,0 +1,23 @@
<launch>
<arg name="user_debug" default="false"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find a1_description)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- for higher robot_state_publisher average rate-->
<!-- <param name="rate" value="1000"/> -->
<!-- send fake joint values -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find a1_description)/launch/check_joint.rviz"/>
</launch>

265
src/robots/a1_description/launch/check_joint.rviz Normal file
View File

@ -0,0 +1,265 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 420
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
trunk:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Value: false
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
Enabled: true
Global Options:
Background Color: 238; 238; 238
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.0307118892669678
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.0760490670800209
Y: 0.11421932280063629
Z: -0.1576911211013794
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.0047969818115234
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.558584690093994
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 627
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000022ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000022f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1108
X: 812
Y: 241

162
src/robots/a1_description/meshes/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

162
src/robots/a1_description/meshes/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

162
src/robots/a1_description/meshes/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

162
src/robots/a1_description/meshes/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

188
src/robots/a1_description/meshes/trunk.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
src/robots/a1_description/meshes/trunk_A1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 70 KiB

14
src/robots/a1_description/package.xml Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<package format="2">
<name>a1_description</name>
<version>0.0.0</version>
<description>The a1_description package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
</package>

973
src/robots/a1_description/urdf/a1.urdf Normal file
View File

@ -0,0 +1,973 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from robot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="a1">
<!-- dynamics inertial value -->
<!-- trunk -->
<!-- <xacro:property name="trunk_mass" value="5.660"/>
<xacro:property name="trunk_com_x" value="0.012731"/>
<xacro:property name="trunk_com_y" value="0.002186"/>
<xacro:property name="trunk_com_z" value="0.000515"/>
<xacro:property name="trunk_ixx" value="0.016839930"/>
<xacro:property name="trunk_ixy" value="0.000083902"/>
<xacro:property name="trunk_ixz" value="0.000597679"/>
<xacro:property name="trunk_iyy" value="0.056579028"/>
<xacro:property name="trunk_iyz" value="0.000025134"/>
<xacro:property name="trunk_izz" value="0.064713601"/> -->
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<!-- ros_control plugin -->
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/a1_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<gazebo>
<plugin filename="libLinkPlot3DPlugin.so" name="3dplotTrunk">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<gazebo>
<plugin filename="libLinkPlot3DPlugin.so" name="3dplot">
<frequency>1000</frequency>
<plot>
<link>FR_calf</link>
<pose>0 0 -0.2 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo> -->
<!-- Visualization of Foot contacts. -->
<!-- <gazebo reference="FR_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo> -->
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- <xacro:include filename="$(find a1_gazebo)/launch/stairs.urdf.xacro"/> -->
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<!-- <xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if> -->
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.267 0.194 0.114"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0041 -0.0005"/>
<mass value="6.0"/>
<inertia ixx="0.0158533" ixy="-3.66e-05" ixz="-6.11e-05" iyy="0.0377999" iyz="-2.75e-05" izz="0.0456542"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1805 -0.047 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003311 -0.000635 3.1e-05"/>
<mass value="0.696"/>
<inertia ixx="0.000469246" ixy="9.409e-06" ixz="-3.42e-07" iyy="0.00080749" iyz="4.66e-07" izz="0.000552929"/>
</inertial>
</link>
<joint name="FR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.081 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FR_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.0838 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003237 0.022327 -0.027326"/>
<mass value="1.013"/>
<inertia ixx="0.005529065" ixy="-4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="-2.2448e-05" izz="0.001367788"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
<mass value="0.166"/>
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="FR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1805 0.047 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003311 0.000635 3.1e-05"/>
<mass value="0.696"/>
<inertia ixx="0.000469246" ixy="-9.409e-06" ixz="-3.42e-07" iyy="0.00080749" iyz="-4.66e-07" izz="0.000552929"/>
</inertial>
</link>
<joint name="FL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.081 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FL_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.0838 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/thigh.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003237 -0.022327 -0.027326"/>
<mass value="1.013"/>
<inertia ixx="0.005529065" ixy="4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="2.2448e-05" izz="0.001367788"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
<mass value="0.166"/>
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="FL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1805 -0.047 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.003311 -0.000635 3.1e-05"/>
<mass value="0.696"/>
<inertia ixx="0.000469246" ixy="-9.409e-06" ixz="3.42e-07" iyy="0.00080749" iyz="4.66e-07" izz="0.000552929"/>
</inertial>
</link>
<joint name="RR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.081 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RR_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.0838 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003237 0.022327 -0.027326"/>
<mass value="1.013"/>
<inertia ixx="0.005529065" ixy="-4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="-2.2448e-05" izz="0.001367788"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
<mass value="0.166"/>
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="RR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1805 0.047 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="21"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.003311 0.000635 3.1e-05"/>
<mass value="0.696"/>
<inertia ixx="0.000469246" ixy="9.409e-06" ixz="3.42e-07" iyy="0.00080749" iyz="-4.66e-07" izz="0.000552929"/>
</inertial>
</link>
<joint name="RL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.081 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RL_thigh_shoulder">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision>
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.0838 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="21"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/thigh.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003237 -0.022327 -0.027326"/>
<mass value="1.013"/>
<inertia ixx="0.005529065" ixy="4.825e-06" ixz="0.000343869" iyy="0.005139339" iyz="2.2448e-05" izz="0.001367788"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.01" friction="0.2"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="21"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006435 0.0 -0.107388"/>
<mass value="0.166"/>
<inertia ixx="0.002997972" ixy="0.0" ixz="-0.000141163" iyy="0.003014022" iyz="0.0" izz="3.2426e-05"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<transmission name="RL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
</robot>

104
src/robots/a1_description/xacro/const.xacro Normal file
View File

@ -0,0 +1,104 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.194"/>
<xacro:property name="trunk_length" value="0.267"/>
<xacro:property name="trunk_height" value="0.114"/>
<xacro:property name="hip_radius" value="0.046"/>
<xacro:property name="hip_length" value="0.04"/>
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
<xacro:property name="thigh_shoulder_length" value="0.032"/>
<xacro:property name="thigh_width" value="0.0245"/>
<xacro:property name="thigh_height" value="0.034"/>
<xacro:property name="calf_width" value="0.016"/>
<xacro:property name="calf_height" value="0.016"/>
<xacro:property name="foot_radius" value="0.02"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.0838"/>
<xacro:property name="thigh_length" value="0.2"/>
<xacro:property name="calf_length" value="0.2"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.1805"/>
<xacro:property name="leg_offset_y" value="0.047"/>
<xacro:property name="trunk_offset_z" value="0.01675"/>
<xacro:property name="hip_offset" value="0.065"/>
<!-- joint limits -->
<xacro:property name="damping" value="0"/>
<xacro:property name="friction" value="0"/>
<xacro:property name="hip_max" value="46"/>
<xacro:property name="hip_min" value="-46"/>
<xacro:property name="hip_velocity_max" value="21"/>
<xacro:property name="hip_torque_max" value="33.5"/>
<xacro:property name="thigh_max" value="240"/>
<xacro:property name="thigh_min" value="-60"/>
<xacro:property name="thigh_velocity_max" value="21"/>
<xacro:property name="thigh_torque_max" value="33.5"/>
<xacro:property name="calf_max" value="-52.5"/>
<xacro:property name="calf_min" value="-154.5"/>
<xacro:property name="calf_velocity_max" value="21"/>
<xacro:property name="calf_torque_max" value="33.5"/>
<!-- dynamics inertial value total 13.9kg -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="6.0"/>
<xacro:property name="trunk_com_x" value="0.0000"/>
<xacro:property name="trunk_com_y" value="0.0041"/>
<xacro:property name="trunk_com_z" value="-0.0005"/>
<xacro:property name="trunk_ixx" value="0.0158533"/>
<xacro:property name="trunk_ixy" value="-0.0000366"/>
<xacro:property name="trunk_ixz" value="-0.0000611"/>
<xacro:property name="trunk_iyy" value="0.0377999"/>
<xacro:property name="trunk_iyz" value="-0.0000275"/>
<xacro:property name="trunk_izz" value="0.0456542"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="0.595"/>
<xacro:property name="hip_com_x" value="-0.003875"/>
<xacro:property name="hip_com_y" value="0.001622"/>
<xacro:property name="hip_com_z" value="0.000042"/>
<xacro:property name="hip_ixx" value="0.000402747"/>
<xacro:property name="hip_ixy" value="-0.000008709"/>
<xacro:property name="hip_ixz" value="-0.000000297"/>
<xacro:property name="hip_iyy" value="0.000691123"/>
<xacro:property name="hip_iyz" value="-0.000000545"/>
<xacro:property name="hip_izz" value="0.000487919"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="0.888"/>
<xacro:property name="thigh_com_x" value="-0.003574"/>
<xacro:property name="thigh_com_y" value="-0.019529"/>
<xacro:property name="thigh_com_z" value="-0.030323"/>
<xacro:property name="thigh_ixx" value="0.005251806"/>
<xacro:property name="thigh_ixy" value="-0.000002168"/>
<xacro:property name="thigh_ixz" value="0.000346889"/>
<xacro:property name="thigh_iyy" value="0.005000475"/>
<xacro:property name="thigh_iyz" value="-0.000028174"/>
<xacro:property name="thigh_izz" value="0.001110200"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.151"/>
<xacro:property name="calf_com_x" value="0.007105"/>
<xacro:property name="calf_com_y" value="-0.000239"/>
<xacro:property name="calf_com_z" value="-0.096933"/>
<xacro:property name="calf_ixx" value="0.002344758"/>
<xacro:property name="calf_ixy" value="0.0"/>
<xacro:property name="calf_ixz" value="-0.000141275"/>
<xacro:property name="calf_iyy" value="0.002360755"/>
<xacro:property name="calf_iyz" value="0.0"/>
<xacro:property name="calf_izz" value="0.000031158"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.06"/>
</robot>

266
src/robots/a1_description/xacro/gazebo.xacro Normal file
View File

@ -0,0 +1,266 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/a1_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<!-- <gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Green</material>
</plot>
</plugin>
</gazebo> -->
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo> -->
<!-- Visualization of Foot contacts. -->
<!-- <gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo> -->
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot>

176
src/robots/a1_description/xacro/leg.xacro Normal file
View File

@ -0,0 +1,176 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find a1_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
<joint name="${name}_hip_joint" type="revolute">
<xacro:insert_block name="origin"/>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_min*PI/180.0}" upper="${hip_max*PI/180.0}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_max*PI/180.0}" upper="${-hip_min*PI/180.0}"/>
</xacro:if>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="package://a1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<joint name="${name}_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 ${(thigh_shoulder_length/2.0+hip_offset)*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="${name}_thigh_shoulder">
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
</geometry>
</collision>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_min*PI/180.0}" upper="${thigh_max*PI/180.0}"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://a1_description/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://a1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_min*PI/180.0}" upper="${calf_max*PI/180.0}"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

40
src/robots/a1_description/xacro/materials.xacro Normal file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

144
src/robots/a1_description/xacro/robot.xacro Normal file
View File

@ -0,0 +1,144 @@
<?xml version="1.0"?>
<robot name="a1" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:include filename="$(find a1_description)/xacro/const.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/leg.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/stairs.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
<!-- <xacro:include filename="$(find a1_gazebo)/launch/stairs.urdf.xacro"/> -->
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rollover Protection mode will add an additional stick on the top, use "true" or "false" to switch it. -->
<xacro:property name="rolloverProtection" value="false"/>
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<xacro:if value="${(rolloverProtection == 'true')}">
<joint name="stick_joint" type="fixed">
<parent link="trunk"/>
<child link="stick_link"/>
<origin rpy="0 0 0" xyz="${0.18} 0 ${stick_length/2.0+0.08}"/>
</joint>
<link name="stick_link">
<visual>
<geometry>
<cylinder length="${stick_length}" radius="${stick_radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length="${stick_length}" radius="${stick_radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="${stick_mass}"/>
<inertia
ixx="${stick_mass / 2.0 * (stick_radius*stick_radius)}" ixy="0.0" ixz="0.0"
iyy="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}" iyz="0.0"
izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/>
</inertial>
</link>
</xacro:if>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:leg>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
</robot>

46
src/robots/a1_description/xacro/stairs.xacro Normal file
View File

@ -0,0 +1,46 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="stair_length" value="0.640" />
<xacro:property name="stair_width" value="0.310" />
<xacro:property name="stair_height" value="0.170" />
<xacro:macro name="stairs" params="stairs xpos ypos zpos">
<joint name="stair_joint_origin" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="stair_link_${stairs}"/>
</joint>
<link name="stair_link_${stairs}">
<visual>
<geometry>
<box size="${stair_length} ${stair_width} ${stair_height}"/>
</geometry>
<material name="grey"/>
<origin rpy="0 0 0" xyz="${xpos} ${ypos} ${zpos}"/>
</visual>
<collision>
<geometry>
<box size="${stair_length} ${stair_width} ${stair_height}"/>
</geometry>
</collision>
<inertial>
<mass value="0.80"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
</inertial>
</link>
<xacro:if value="${stairs}">
<xacro:stairs stairs="${stairs-1}" xpos="0" ypos="${ypos-stair_width/2}" zpos="${zpos+stair_height}"/>
<joint name="stair_joint_${stairs}" type="fixed">
<parent link="stair_link_${stairs}"/>
<child link="stair_link_${stairs-1}"/>
</joint>
</xacro:if>
</xacro:macro>
</robot>

42
src/robots/a1_description/xacro/transmission.xacro Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

24
src/robots/cyberdog1_description/config/robot_control.yaml
View File

@ -6,65 +6,65 @@ cyberdog_gazebo:
# FL Controllers --------------------------------------- # FL Controllers ---------------------------------------
FL_hip_controller: FL_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FL_hip_joint joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller: FL_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller: FL_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FL_calf_joint joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers --------------------------------------- # FR Controllers ---------------------------------------
FR_hip_controller: FR_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FR_hip_joint joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller: FR_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller: FR_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FR_calf_joint joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers --------------------------------------- # RL Controllers ---------------------------------------
RL_hip_controller: RL_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RL_hip_joint joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller: RL_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller: RL_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RL_calf_joint joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers --------------------------------------- # RR Controllers ---------------------------------------
RR_hip_controller: RR_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RR_hip_joint joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller: RR_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller: RR_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RR_calf_joint joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}

8
src/robots/cyberdog1_description/urdf/cyberdog_description.urdf
View File

@ -59,7 +59,7 @@
</sensor> </sensor>
</gazebo> </gazebo>
<!-- Foot contacts. --> <!-- Foot contacts. -->
<gazebo reference="FR_calf"> <!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact"> <sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate> <update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/> <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
@ -94,9 +94,9 @@
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision> <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact> </contact>
</sensor> </sensor>
</gazebo> </gazebo> -->
<!-- Visualization of Foot contacts. --> <!-- Visualization of Foot contacts. -->
<gazebo reference="FR_foot"> <!-- <gazebo reference="FR_foot">
<visual> <visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin"> <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>FR_foot_contact</topicName> <topicName>FR_foot_contact</topicName>
@ -123,7 +123,7 @@
<topicName>RL_foot_contact</topicName> <topicName>RL_foot_contact</topicName>
</plugin> </plugin>
</visual> </visual>
</gazebo> </gazebo> -->
<gazebo reference="base"> <gazebo reference="base">
<material>Gazebo/Green</material> <material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>

8
src/robots/cyberdog1_description/xacro/gazebo.xacro
View File

@ -60,7 +60,7 @@
</gazebo> </gazebo>
<!-- Foot contacts. --> <!-- Foot contacts. -->
<gazebo reference="FR_calf"> <!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact"> <sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate> <update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/> <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
@ -95,10 +95,10 @@
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision> <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact> </contact>
</sensor> </sensor>
</gazebo> </gazebo> -->
<!-- Visualization of Foot contacts. --> <!-- Visualization of Foot contacts. -->
<gazebo reference="FR_foot"> <!-- <gazebo reference="FR_foot">
<visual> <visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so"> <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName> <topicName>FR_foot_contact</topicName>
@ -125,7 +125,7 @@
<topicName>RL_foot_contact</topicName> <topicName>RL_foot_contact</topicName>
</plugin> </plugin>
</visual> </visual>
</gazebo> </gazebo> -->
<gazebo reference="base"> <gazebo reference="base">
<material>Gazebo/Green</material> <material>Gazebo/Green</material>

24
src/robots/cyberdog_old_description/config/robot_control.yaml
View File

@ -6,65 +6,65 @@ cyberdog_gazebo:
# FL Controllers --------------------------------------- # FL Controllers ---------------------------------------
FL_hip_controller: FL_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FL_hip_joint joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller: FL_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller: FL_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FL_calf_joint joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers --------------------------------------- # FR Controllers ---------------------------------------
FR_hip_controller: FR_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FR_hip_joint joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller: FR_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller: FR_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: FR_calf_joint joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers --------------------------------------- # RL Controllers ---------------------------------------
RL_hip_controller: RL_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RL_hip_joint joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller: RL_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller: RL_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RL_calf_joint joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers --------------------------------------- # RR Controllers ---------------------------------------
RR_hip_controller: RR_hip_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RR_hip_joint joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0} pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller: RR_thigh_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller: RR_calf_controller:
type: unitree_legged_control/UnitreeJointController type: robot_joint_controller/RobotJointController
joint: RR_calf_joint joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0} pid: {p: 300.0, i: 0.0, d: 8.0}

8
src/robots/cyberdog_old_description/urdf/cyberdog_old_description.urdf
View File

@ -84,7 +84,7 @@
</sensor> </sensor>
</gazebo> </gazebo>
<!-- Foot contacts. --> <!-- Foot contacts. -->
<gazebo reference="FR_calf"> <!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact"> <sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate> <update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/> <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
@ -119,9 +119,9 @@
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision> <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact> </contact>
</sensor> </sensor>
</gazebo> </gazebo> -->
<!-- Visualization of Foot contacts. --> <!-- Visualization of Foot contacts. -->
<gazebo reference="FR_foot"> <!-- <gazebo reference="FR_foot">
<visual> <visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin"> <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>FR_foot_contact</topicName> <topicName>FR_foot_contact</topicName>
@ -148,7 +148,7 @@
<topicName>RL_foot_contact</topicName> <topicName>RL_foot_contact</topicName>
</plugin> </plugin>
</visual> </visual>
</gazebo> </gazebo> -->
<gazebo reference="base"> <gazebo reference="base">
<material>Gazebo/Green</material> <material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff> <turnGravityOff>false</turnGravityOff>

1
src/unitree_ros

@ -1 +0,0 @@
Subproject commit 67ac2124494e93af87d34d6e53ef9060dbf9c923