first draft version
This commit is contained in:
Ray
14
RobotOS1/CMakeLists.txt
Normal file
14
RobotOS1/CMakeLists.txt
Normal file
@@ -0,0 +1,14 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
|
||||
project(robot3)
|
||||
|
||||
find_package(catkin REQUIRED)
|
||||
|
||||
catkin_package()
|
||||
|
||||
find_package(roslaunch)
|
||||
|
||||
foreach(dir config launch meshes urdf)
|
||||
install(DIRECTORY ${dir}/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
||||
endforeach(dir)
|
||||
1
RobotOS1/config/joint_names_robot3.yaml
Normal file
1
RobotOS1/config/joint_names_robot3.yaml
Normal file
@@ -0,0 +1 @@
|
||||
controller_joint_names: ['', 'left_hip_pitch_joint', 'left_leg_pitch_joint', 'left_wheel2_joint', 'left_wheel1_joint', 'right_hip_pitch_joint', 'right_leg_pitch_joint', 'right_wheel2_joint', 'right_wheel1_joint', 'left_shoulder_joint', 'left_arm1_joint', 'left_arm2_joint', 'right_shoulder_joint', 'right_arm1_joint', 'right_arm2_joint', ]
|
||||
1147
RobotOS1/export.log
Normal file
1147
RobotOS1/export.log
Normal file
File diff suppressed because one or more lines are too long
20
RobotOS1/launch/display.launch
Normal file
20
RobotOS1/launch/display.launch
Normal file
@@ -0,0 +1,20 @@
|
||||
<launch>
|
||||
<arg
|
||||
name="model" />
|
||||
<param
|
||||
name="robot_description"
|
||||
textfile="$(find robot3)/urdf/robot3.urdf" />
|
||||
<node
|
||||
name="joint_state_publisher_gui"
|
||||
pkg="joint_state_publisher_gui"
|
||||
type="joint_state_publisher_gui" />
|
||||
<node
|
||||
name="robot_state_publisher"
|
||||
pkg="robot_state_publisher"
|
||||
type="robot_state_publisher" />
|
||||
<node
|
||||
name="rviz"
|
||||
pkg="rviz"
|
||||
type="rviz"
|
||||
args="-d $(find robot3)/urdf.rviz" />
|
||||
</launch>
|
||||
20
RobotOS1/launch/gazebo.launch
Normal file
20
RobotOS1/launch/gazebo.launch
Normal file
@@ -0,0 +1,20 @@
|
||||
<launch>
|
||||
<include
|
||||
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
||||
<node
|
||||
name="tf_footprint_base"
|
||||
pkg="tf"
|
||||
type="static_transform_publisher"
|
||||
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||
<node
|
||||
name="spawn_model"
|
||||
pkg="gazebo_ros"
|
||||
type="spawn_model"
|
||||
args="-file $(find robot3)/urdf/robot3.urdf -urdf -model robot3"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
</launch>
|
||||
BIN
RobotOS1/meshes/base_link.STL
Normal file
BIN
RobotOS1/meshes/base_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_arm1_link.STL
Normal file
BIN
RobotOS1/meshes/left_arm1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_arm2_link.STL
Normal file
BIN
RobotOS1/meshes/left_arm2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_hip_pitch_link.STL
Normal file
BIN
RobotOS1/meshes/left_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_leg_pitch_link.STL
Normal file
BIN
RobotOS1/meshes/left_leg_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_shoulder_link.STL
Normal file
BIN
RobotOS1/meshes/left_shoulder_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_wheel1_link.STL
Normal file
BIN
RobotOS1/meshes/left_wheel1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/left_wheel2_link.STL
Normal file
BIN
RobotOS1/meshes/left_wheel2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_arm1_link.STL
Normal file
BIN
RobotOS1/meshes/right_arm1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_arm2_link.STL
Normal file
BIN
RobotOS1/meshes/right_arm2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_hip_pitch_link.STL
Normal file
BIN
RobotOS1/meshes/right_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_leg_pitch_link.STL
Normal file
BIN
RobotOS1/meshes/right_leg_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_shoulder_link.STL
Normal file
BIN
RobotOS1/meshes/right_shoulder_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_wheel1_link.STL
Normal file
BIN
RobotOS1/meshes/right_wheel1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/meshes/right_wheel2_link.STL
Normal file
BIN
RobotOS1/meshes/right_wheel2_link.STL
Normal file
Binary file not shown.
21
RobotOS1/package.xml
Normal file
21
RobotOS1/package.xml
Normal file
@@ -0,0 +1,21 @@
|
||||
<package format="2">
|
||||
<name>robot3</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
<p>URDF Description package for robot3</p>
|
||||
<p>This package contains configuration data, 3D models and launch files
|
||||
for robot3 robot</p>
|
||||
</description>
|
||||
<author>TODO</author>
|
||||
<maintainer email="TODO@email.com" />
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roslaunch</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
<depend>rviz</depend>
|
||||
<depend>joint_state_publisher_gui</depend>
|
||||
<depend>gazebo</depend>
|
||||
<export>
|
||||
<architecture_independent />
|
||||
</export>
|
||||
</package>
|
||||
BIN
RobotOS1/urdf/base_link.STL
Normal file
BIN
RobotOS1/urdf/base_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_arm1_link.STL
Normal file
BIN
RobotOS1/urdf/left_arm1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_arm2_link.STL
Normal file
BIN
RobotOS1/urdf/left_arm2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_hip_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/left_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_leg_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/left_leg_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_shoulder_link.STL
Normal file
BIN
RobotOS1/urdf/left_shoulder_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_wheel1_link.STL
Normal file
BIN
RobotOS1/urdf/left_wheel1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/left_wheel2_link.STL
Normal file
BIN
RobotOS1/urdf/left_wheel2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/base_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/base_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_arm1_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_arm1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_arm2_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_arm2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_hip_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_leg_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_leg_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_shoulder_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_shoulder_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_wheel1_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_wheel1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/left_wheel2_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/left_wheel2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_arm1_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_arm1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_arm2_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_arm2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_hip_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_leg_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_leg_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_shoulder_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_shoulder_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_wheel1_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_wheel1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/meshes/right_wheel2_link.STL
Normal file
BIN
RobotOS1/urdf/meshes/right_wheel2_link.STL
Normal file
Binary file not shown.
152
RobotOS1/urdf/meshes/robot_roll.py
Normal file
152
RobotOS1/urdf/meshes/robot_roll.py
Normal file
@@ -0,0 +1,152 @@
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import time
|
||||
import math
|
||||
|
||||
# 目标Roll倾斜角度（单位：度）
|
||||
TARGET_ROLL_DEGREE = 7.0 # 目标倾斜7度
|
||||
ROLL_TRANSITION_DURATION = 1.5 # 倾斜过渡时间（秒）
|
||||
STABLE_DURATION = 1.0 # 稳定阶段持续时间（秒）
|
||||
|
||||
def main():
|
||||
# 加载模型和创建模拟
|
||||
model = mujoco.MjModel.from_xml_path("/home/mrji/mujoco_learning/robot3/urdf/robot3.xml")
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
# 完整重置机器人状态（第一次重置）
|
||||
mujoco.mj_resetData(model, data)
|
||||
|
||||
# 设置初始关节角度为0
|
||||
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
|
||||
|
||||
# 重置所有速度为0
|
||||
data.qvel[:] = 0
|
||||
|
||||
# 第二次重置模型数据（规避加载问题）
|
||||
mujoco.mj_resetData(model, data)
|
||||
|
||||
# 再次设置初始关节角度为0
|
||||
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
|
||||
|
||||
# 创建查看器
|
||||
with mujoco.viewer.launch_passive(model, data) as viewer:
|
||||
# 设置摄像头位置
|
||||
viewer.cam.azimuth = 180 # 面向机器人正面
|
||||
viewer.cam.elevation = -20 # 稍微俯视
|
||||
viewer.cam.distance = 2.0 # 距离
|
||||
viewer.cam.lookat[:] = [0.0, 0.0, 0.65] # 看向机器人中心
|
||||
|
||||
# 获取执行器ID
|
||||
hip_left = model.actuator("1_pos").id
|
||||
leg_left = model.actuator("2_pos").id
|
||||
hip_right = model.actuator("3_pos").id
|
||||
leg_right = model.actuator("4_pos").id
|
||||
|
||||
# 设置控制信号初始值为0
|
||||
data.ctrl[hip_left] = 0
|
||||
data.ctrl[leg_left] = 0
|
||||
data.ctrl[hip_right] = 0
|
||||
data.ctrl[leg_right] = 0
|
||||
|
||||
# 修改：在最开始的时候额外重置一次
|
||||
mujoco.mj_resetData(model, data)
|
||||
|
||||
# 状态控制变量
|
||||
start_time = time.time()
|
||||
current_phase = "STABLE" # STABLE, MOVING, DONE
|
||||
transition_start_time = 0
|
||||
|
||||
# 控制参数
|
||||
hip_adjustment = 0.1 # 髋关节调整量（根据实验确定）
|
||||
leg_adjustment = 0.15 # 腿关节调整量（根据实验确定）
|
||||
initial_roll = 0.0 # 初始Roll角度
|
||||
current_roll = 0.0 # 当前Roll角度
|
||||
|
||||
# 初始稳定步骤 - 让物理引擎稳定
|
||||
for _ in range(200):
|
||||
mujoco.mj_step(model, data)
|
||||
viewer.sync()
|
||||
time.sleep(0.001)
|
||||
|
||||
# 模拟循环
|
||||
while viewer.is_running():
|
||||
step_start = time.time()
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
# 状态机控制
|
||||
if current_phase == "STABLE":
|
||||
# 稳定阶段 - 保持初始姿态
|
||||
if elapsed_time >= STABLE_DURATION:
|
||||
# 记录初始姿态（Roll角度）
|
||||
quat = data.sensor("orientation").data.copy()
|
||||
initial_roll = get_roll_from_quat(quat)
|
||||
print(f"稳定阶段结束，初始Roll角度: {math.degrees(initial_roll):.2f}度")
|
||||
|
||||
current_phase = "MOVING"
|
||||
transition_start_time = time.time()
|
||||
print("开始Roll倾斜过渡...")
|
||||
|
||||
elif current_phase == "MOVING":
|
||||
# 过渡阶段 - 平滑过渡到目标倾斜
|
||||
transition_elapsed = time.time() - transition_start_time
|
||||
|
||||
if transition_elapsed >= ROLL_TRANSITION_DURATION:
|
||||
# 达到目标倾斜
|
||||
data.ctrl[hip_left] = 0 - hip_adjustment
|
||||
data.ctrl[leg_left] = 0 - leg_adjustment
|
||||
data.ctrl[hip_right] = 0 + hip_adjustment
|
||||
data.ctrl[leg_right] = 0 + leg_adjustment
|
||||
|
||||
current_phase = "DONE"
|
||||
print("达到目标倾斜状态")
|
||||
else:
|
||||
# 线性插值过渡
|
||||
fraction = transition_elapsed / ROLL_TRANSITION_DURATION
|
||||
data.ctrl[hip_left] = 0 - hip_adjustment * fraction
|
||||
data.ctrl[leg_left] = 0 - leg_adjustment * fraction
|
||||
data.ctrl[hip_right] = 0 + hip_adjustment * fraction
|
||||
data.ctrl[leg_right] = 0 + leg_adjustment * fraction
|
||||
|
||||
elif current_phase == "DONE":
|
||||
# 倾斜完成阶段 - 保持当前状态
|
||||
# 计算并输出当前Roll角度
|
||||
quat = data.sensor("orientation").data.copy()
|
||||
current_roll = get_roll_from_quat(quat)
|
||||
roll_angle_deg = math.degrees(current_roll - initial_roll)
|
||||
print(f"当前Roll倾斜角度: {roll_angle_deg:.2f}度")
|
||||
|
||||
# 停留一段时间后退出
|
||||
if time.time() - transition_start_time > ROLL_TRANSITION_DURATION + 2.0:
|
||||
print(f"最终Roll倾斜角度: {roll_angle_deg:.2f}度 (目标: {TARGET_ROLL_DEGREE}度)")
|
||||
break
|
||||
|
||||
# 执行物理模拟
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
# 同步查看器
|
||||
viewer.sync()
|
||||
|
||||
# 控制步长（约1000Hz）
|
||||
time_until_next_step = 0.001 - (time.time() - step_start)
|
||||
if time_until_next_step > 0:
|
||||
time.sleep(time_until_next_step)
|
||||
|
||||
def get_roll_from_quat(quat):
|
||||
"""从四元数中提取Roll角度（弧度）"""
|
||||
# 四元数转换为旋转矩阵
|
||||
rot_matrix = np.zeros(9)
|
||||
mujoco.mju_quat2Mat(rot_matrix, quat)
|
||||
|
||||
# 从旋转矩阵中提取Roll角度（绕X轴的旋转）
|
||||
roll = math.atan2(rot_matrix[7], rot_matrix[8])
|
||||
return roll
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
BIN
RobotOS1/urdf/right_arm1_link.STL
Normal file
BIN
RobotOS1/urdf/right_arm1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/right_arm2_link.STL
Normal file
BIN
RobotOS1/urdf/right_arm2_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/right_hip_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/right_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/right_leg_pitch_link.STL
Normal file
BIN
RobotOS1/urdf/right_leg_pitch_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/right_shoulder_link.STL
Normal file
BIN
RobotOS1/urdf/right_shoulder_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/right_wheel1_link.STL
Normal file
BIN
RobotOS1/urdf/right_wheel1_link.STL
Normal file
Binary file not shown.
BIN
RobotOS1/urdf/right_wheel2_link.STL
Normal file
BIN
RobotOS1/urdf/right_wheel2_link.STL
Normal file
Binary file not shown.
16
RobotOS1/urdf/robot3.csv
Normal file
16
RobotOS1/urdf/robot3.csv
Normal file
@@ -0,0 +1,16 @@
|
||||
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||
base_link,-0.0598891820748674,5.31626531352636E-07,0.164869219254393,0,0,0,7.33969273744462,0.0353291392023482,-2.8945750194243E-08,2.11594095208159E-06,0.0335711592208216,0.000271762665669396,0.00701126241700792,0,0,0,0,0,0,package://robot3/meshes/base_link.STL,0.666666666666667,0.666666666666667,0.666666666666667,1,0,0,0,0,0,0,package://robot3/meshes/base_link.STL,,新上半身另一半-2;肩膀下电机简化-1;支撑板-1;肩膀下电机简化-2;支撑板-2;Angle For Aluminum Profile 3030-3;Angle For Aluminum Profile 3030-5;Angle For Aluminum Profile 3030-1;Angle For Aluminum Profile 3030-4;Sealed-Lead-Acid-12V-1.3Ah-1;电池连接件1-1;Sealed-Lead-Acid-12V-1.3Ah-2;电池连接件1-2;简化轴承座-2;简化轴承座-1;AA简化新上半身-1/新左腰部平台-1;AA简化新上半身-1/新右腰部平台-1;插销推杆-2;定位模块2-2;定位模块-1;六角插销-3;新上半身一半-1;光轴-3;光轴-2;简化轴承座-3;简化轴承座-4;右髋-1/右侧电机座-1;左髋-1/左侧电机座-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||
left_hip_pitch_link,3.93767161736252E-06,0.161490126813208,0.0158567790568734,0,0,0,0.647526339814874,0.0102648545860372,-2.29409678904373E-06,-2.59558334421073E-08,0.000246501135769241,1.67258066439869E-05,0.010329535938566,0,0,0,0,0,0,package://robot3/meshes/left_hip_pitch_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/left_hip_pitch_link.STL,,简化电机-1;内侧腿-2/右内腿板-1;外侧腿-2/内腿连接法兰B-1,left_hip_pitch,A_left_hip_pitch,left_hip_pitch_joint,revolute,-0.06,-0.1358,-0.037269,-1.5708,-0.0095732,0,base_link,0,0,-1,100,50,-1.57,1.57,,,,,,,,
|
||||
left_leg_pitch_link,-2.63464180747426E-05,0.197159145100908,0.000498203326348359,0,0,0,0.512633617146925,0.00722923341588236,2.08332581819351E-06,8.43234132073939E-09,0.000184537126972943,-8.42677914064545E-05,0.00733154966415246,0,0,0,0,0,0,package://robot3/meshes/left_leg_pitch_link.STL,1,0.372549019607843,0.294117647058824,1,0,0,0,0,0,0,package://robot3/meshes/left_leg_pitch_link.STL,,外侧腿-2/腿板连接件B-1;外侧腿-2/端盖B-1;外侧腿-2/腿板_外侧1-1,left_leg_pitch,A_left_leg_pitch,left_leg_pitch_joint,revolute,0,0,-0.0295,0,0,0.01205,left_hip_pitch_link,0,0,1,100,50,-1.57,1.57,,,,,,,,
|
||||
left_wheel2_link,4.1585564079967E-08,-5.47447156262759E-06,0.0187838263233595,0,0,0,0.246221197660763,0.000155862515432955,-7.05176250297316E-11,2.43528481620933E-10,0.000155872043160321,-3.20589554855461E-08,0.000262570842948365,0,0,0,0,0,0,package://robot3/meshes/left_wheel2_link.STL,0.780392156862745,0.780392156862745,0.780392156862745,1,0,0,0,0,0,0,package://robot3/meshes/left_wheel2_link.STL,,外侧腿-2/轮毂电机-1,left_wheel2,A_left_wheel2,left_wheel2_joint,revolute,0,0.4805,0.004,-3.1416,0,-0.0024764,left_leg_pitch_link,0,0,1,100,50,-1000,1000,,,,,,,,
|
||||
left_wheel1_link,1.97042367054093E-06,5.10773910927798E-06,0.0187838263233595,0,0,0,0.246221197660763,0.000155863750535229,3.20112680897945E-09,1.15389629880907E-08,0.000155870808058046,2.99113400920877E-08,0.000262570842948365,0,0,0,0,0,0,package://robot3/meshes/left_wheel1_link.STL,0.780392156862745,0.780392156862745,0.780392156862745,1,0,0,0,0,0,0,package://robot3/meshes/left_wheel1_link.STL,,内侧腿-2/轮毂电机-1,left_wheel1,A_left_wheel1,left_wheel1_joint,revolute,0,0.48,0.01,0,0,0.0095732,left_hip_pitch_link,0,0,1,100,50,-1000,1000,,,,,,,,
|
||||
right_hip_pitch_link,-3.97209815898069E-06,-0.161490067811833,0.0158567790568734,0,0,0,0.647526339814875,0.0102648550884178,-2.29438072118385E-06,2.6182761858173E-08,0.000246500633388666,-1.6726195561603E-05,0.010329535938566,0,0,0,0,0,0,package://robot3/meshes/right_hip_pitch_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/right_hip_pitch_link.STL,,简化电机-2;外侧腿-1/内腿连接法兰B-1;内侧腿-1/右内腿板-1,right_hip_pitch,A_right_hip_pitch,right_hip_pitch_joint,revolute,-0.06,0.1358,-0.037269,1.5708,-0.0029091,0,base_link,0,0,-1,100,50,-1.57,1.57,,,,,,,,
|
||||
right_leg_pitch_link,3.43008600186093E-05,-0.197159143801203,0.000498203308628542,0,0,0,0.512633618713017,0.00722923325218376,2.36753175021739E-06,-1.18316332479498E-08,0.000184537306689484,8.42677920023009E-05,0.00733154968000536,0,0,0,0,0,0,package://robot3/meshes/right_leg_pitch_link.STL,0.647058823529412,0.619607843137255,0.588235294117647,1,0,0,0,0,0,0,package://robot3/meshes/right_leg_pitch_link.STL,,外侧腿-1/端盖B-1;外侧腿-1/腿板连接件B-1;外侧腿-1/腿板_外侧1-1,right_leg_pitch,A_right_leg_pitch,right_leg_pitch_joint,revolute,0,0,-0.0295,0,0,-0.0029091,right_hip_pitch_link,0,0,1,100,50,-1.57,1.57,,,,,,,,
|
||||
right_wheel2_link,-5.49216228909084E-08,5.47435401360152E-06,0.0187838263233596,0,0,0,0.246221197660763,0.000155862515833067,-9.37269060893747E-11,-3.2162553827427E-10,0.000155872042760209,3.2058267107854E-08,0.000262570842948364,0,0,0,0,0,0,package://robot3/meshes/right_wheel2_link.STL,0.780392156862745,0.780392156862745,0.780392156862745,1,0,0,0,0,0,0,package://robot3/meshes/right_wheel2_link.STL,,外侧腿-1/轮毂电机-1,right_wheel2,A_right_wheel2,right_wheel2_joint,revolute,1.9386E-05,-0.4805,0.004,3.1416,0,0,right_leg_pitch_link,0,0,1,100,50,-1000,1000,,,,,,,,
|
||||
right_wheel1_link,-1.90651569380871E-06,-5.13193590762073E-06,0.0187838263233596,0,0,0,0.246221197660763,0.000155863671728484,3.11204465575566E-09,-1.11647126229566E-08,0.000155870886864792,-3.00530385317656E-08,0.000262570842948364,0,0,0,0,0,0,package://robot3/meshes/right_wheel1_link.STL,0.780392156862745,0.780392156862745,0.780392156862745,1,0,0,0,0,0,0,package://robot3/meshes/right_wheel1_link.STL,,内侧腿-1/轮毂电机-1,right_wheel1,A_right_wheel1,right_wheel1_joint,revolute,0,-0.48,0.01,0,0,-0.0029091,right_hip_pitch_link,0,0,1,100,50,-1000,1000,,,,,,,,
|
||||
left_shoulder_link,-0.0144837915216556,0.000108169744531657,0.0209530786562939,0,0,0,0.0131460728071447,6.97527886815648E-06,6.50120079293974E-10,1.71422653747178E-06,7.0623242664372E-06,-1.28024106360931E-08,4.51847500288221E-06,0,0,0,0,0,0,package://robot3/meshes/left_shoulder_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/left_shoulder_link.STL,,肩部上下电机连-2,left_shoulder,A_left_shoulder,left_shoulder_joint,revolute,-0.06,-0.0878,0.36288,0,0,0,base_link,0,0,1,100,50,-1.57,1.57,,,,,,,,
|
||||
left_arm1_link,-0.00228631297282665,-0.000694770956028379,0.0248681244469774,0,0,0,0.205368959445536,0.000453271756683533,4.86050519285356E-07,1.78313281804853E-06,0.000439236029042774,-2.38826303979722E-06,5.94852101938372E-05,0,0,0,0,0,0,package://robot3/meshes/left_arm1_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/left_arm1_link.STL,,肩膀上电机简化-1;arm1_bendlink-3;3rd(i)-8,left_arm1,A_left_arm1,left_arm1_joint,revolute,0,0,0.0385,1.5708,0,-0.0074682,left_shoulder_link,-1,0,0,42.8,50,-1.5,1.5,,,,,,,,
|
||||
left_arm2_link,-0.0237485342372672,-0.0546317049801525,1.0877545797372E-05,0,0,0,0.224709252335196,0.000216521591384588,-1.59232055667217E-06,1.95924741151164E-09,4.6574554279622E-05,-8.90668315983443E-08,0.000226443044997353,0,0,0,0,0,0,package://robot3/meshes/left_arm2_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/left_arm2_link.STL,,DC13_X540_DUMMY-3;4th(i)-7;DC13_X540_DUMMY-1;4th(i)-9,left_arm2,A_left_arm2,left_arm2_joint,revolute,0.020127,0.0040341,0.17817,-1.6062,0.0074682,0,left_arm1_link,-0.99997,0.0074634,0.0002647,18,50,-1.5,1.5,,,,,,,,
|
||||
right_shoulder_link,-0.0144831349305154,-0.000175271563297497,0.0209530786562937,0,0,0,0.0131460728071447,6.97528676044484E-06,-1.0533666096497E-09,1.71414882674561E-06,7.06231637414882E-06,2.07442343132162E-08,4.51847500288222E-06,0,0,0,0,0,0,package://robot3/meshes/right_shoulder_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/right_shoulder_link.STL,,肩部上下电机连-1,right_shoulder,A_right_shoulder,right_shoulder_joint,revolute,-0.06,0.0878,0.36288,0,0,0,base_link,0,0,1,100,50,-1.57,1.57,,,,,,,,
|
||||
right_arm1_link,-0.00258654183724675,0.00163071193731268,-0.0247948407356193,0,0,0,0.205368959445536,0.000453170582378804,-2.74429063189701E-07,-6.56011918750897E-06,0.000438877289766572,1.19124523787236E-05,5.99451237747681E-05,0,0,0,0,0,0,package://robot3/meshes/right_arm1_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/right_arm1_link.STL,,arm1_bendlink-1;3rd(i)-7;肩膀上电机简化-2,right_arm1,A_right_arm1,right_arm1_joint,revolute,0,0,0.0385,1.5708,0,0,right_shoulder_link,1,0,0,42.8,50,-1.5,1.5,,,,,,,,
|
||||
right_arm2_link,-0.0240175529876588,0.0558375631647781,2.11385309717427E-06,0,0,0,0.224709252312172,0.000216503168895651,2.38031155628203E-06,5.65361793210531E-11,4.65929763038616E-05,-8.9238704186976E-08,0.000226443045453438,0,0,0,0,0,0,package://robot3/meshes/right_arm2_link.STL,1,1,1,1,0,0,0,0,0,0,package://robot3/meshes/right_arm2_link.STL,,DC13_X540_DUMMY-2;4th(i)-10;4th(i)-8;DC13_X540_DUMMY-4,right_arm2,A_right_arm2,right_arm2_joint,revolute,0.017969,0.0026972,-0.17843,-1.5793,0,0,right_arm1_link,-0.99993,-0.0121,-0.00010316,18,50,-1.5,1.5,,,,,,,,
|
||||
|
866
RobotOS1/urdf/robot3.urdf
Normal file
866
RobotOS1/urdf/robot3.urdf
Normal file
@@ -0,0 +1,866 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="robot3">
|
||||
<mujoco>
|
||||
<compiler
|
||||
meshdir="/home/mrji/mujoco_learning/robot3/meshes"
|
||||
balanceinertia="true"
|
||||
discardvisual="false" />
|
||||
</mujoco>
|
||||
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0598891820748674 5.31626531352636E-07 0.164869219254393"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="5.4247767" />
|
||||
<inertia
|
||||
ixx="0.1131876"
|
||||
ixy="0.0000008"
|
||||
ixz="0.0001546"
|
||||
iyy="0.0858545"
|
||||
iyz="0.0000006"
|
||||
izz="0.0325040" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.666666666666667 0.666666666666667 0.666666666666667 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="left_hip_pitch_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="3.93767161736252E-06 0.161490126813208 0.0158567790568734"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.6475263" />
|
||||
<inertia
|
||||
ixx="0.0182516"
|
||||
ixy="0.0000058"
|
||||
ixz="0.0001729"
|
||||
iyy="0.0181748"
|
||||
iyz="-0.0006055"
|
||||
izz="0.0003913" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_hip_pitch_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_hip_pitch_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_hip_pitch_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.06 -0.1358 -0.037269"
|
||||
rpy="-1.5708 -0.0095732 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="left_hip_pitch_link" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_leg_pitch_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-2.63464180747426E-05 0.197159145100908 0.000498203326348359"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.5126336" />
|
||||
<inertia
|
||||
ixx="0.0135141"
|
||||
ixy="0.0000005"
|
||||
ixz="-0.0000325"
|
||||
iyy="0.0135986"
|
||||
iyz="0.0001480"
|
||||
izz="0.0002025" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_leg_pitch_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 0.372549019607843 0.294117647058824 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_leg_pitch_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_leg_pitch_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.0295"
|
||||
rpy="0 0 0.01205" />
|
||||
<parent
|
||||
link="left_hip_pitch_link" />
|
||||
<child
|
||||
link="left_leg_pitch_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_wheel2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.745" />
|
||||
<inertia
|
||||
ixx="0.0004716"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0007945"
|
||||
iyz="0.0000001"
|
||||
izz="0.0004716" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_wheel2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_wheel2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_wheel2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0.4805 0.004"
|
||||
rpy="-3.1416 0 -0.0024764" />
|
||||
<parent
|
||||
link="left_leg_pitch_link" />
|
||||
<child
|
||||
link="left_wheel2_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1000"
|
||||
upper="1000"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_wheel1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.745" />
|
||||
<inertia
|
||||
ixx="0.0004716"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0007945"
|
||||
iyz="0.0000001"
|
||||
izz="0.0004716" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_wheel1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_wheel1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_wheel1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0.48 0.01"
|
||||
rpy="0 0 0.0095732" />
|
||||
<parent
|
||||
link="left_hip_pitch_link" />
|
||||
<child
|
||||
link="left_wheel1_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1000"
|
||||
upper="1000"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_hip_pitch_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.6475263" />
|
||||
<inertia
|
||||
ixx="0.0182531"
|
||||
ixy="-0.0000017"
|
||||
ixz="0.0000501"
|
||||
iyy="0.0181748"
|
||||
iyz="0.0006055"
|
||||
izz="0.0003897" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_hip_pitch_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_hip_pitch_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_hip_pitch_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.06 0.1358 -0.037269"
|
||||
rpy="1.5708 -0.0029091 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="right_hip_pitch_link" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_leg_pitch_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="3.43008600186093E-05 -0.197159143801203 0.000498203308628542"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.5126336" />
|
||||
<inertia
|
||||
ixx="0.0135142"
|
||||
ixy="0.0000001"
|
||||
ixz="0.0000001"
|
||||
iyy="0.0135986"
|
||||
iyz="-0.0001480"
|
||||
izz="0.0002024" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_leg_pitch_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_leg_pitch_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_leg_pitch_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.0295"
|
||||
rpy="0 0 -0.0029091" />
|
||||
<parent
|
||||
link="right_hip_pitch_link" />
|
||||
<child
|
||||
link="right_leg_pitch_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_wheel2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.745" />
|
||||
<inertia
|
||||
ixx="0.0004716"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0007945"
|
||||
iyz="0.0000001"
|
||||
izz="0.0004716" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_wheel2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_wheel2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_wheel2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="1.9386E-05 -0.4805 0.004"
|
||||
rpy="3.1416 0 0" />
|
||||
<parent
|
||||
link="right_leg_pitch_link" />
|
||||
<child
|
||||
link="right_wheel2_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1000"
|
||||
upper="1000"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_wheel1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.745" />
|
||||
<inertia
|
||||
ixx="0.0004716"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0007945"
|
||||
iyz="0.0000001"
|
||||
izz="0.0004716" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_wheel1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.780392156862745 0.780392156862745 0.780392156862745 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_wheel1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_wheel1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.48 0.01"
|
||||
rpy="0 0 -0.0029091" />
|
||||
<parent
|
||||
link="right_hip_pitch_link" />
|
||||
<child
|
||||
link="right_wheel1_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1000"
|
||||
upper="1000"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_shoulder_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0144837915216556 0.000108169744531657 0.0209530786562939"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0131461" />
|
||||
<inertia
|
||||
ixx="0.0000070"
|
||||
ixy="0"
|
||||
ixz="0.0000017"
|
||||
iyy="0.0000071"
|
||||
iyz="0"
|
||||
izz="0.0000045" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_shoulder_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_shoulder_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_shoulder_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.06 -0.0878 0.36288"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="left_shoulder_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00228631297282665 -0.000694770956028379 0.0248681244469774"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.5854363" />
|
||||
<inertia
|
||||
ixx="0.0008783"
|
||||
ixy="-0.0000561"
|
||||
ixz="0.0000086"
|
||||
iyy="0.0002821"
|
||||
iyz="-0.0000017"
|
||||
izz="0.0009299" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_arm1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_arm1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.0385"
|
||||
rpy="1.5708 0 -0.0074682" />
|
||||
<parent
|
||||
link="left_shoulder_link" />
|
||||
<child
|
||||
link="left_arm1_link" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="-1.5"
|
||||
upper="1.5"
|
||||
effort="42.8"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.7955302" />
|
||||
<inertia
|
||||
ixx="0.0035648"
|
||||
ixy="-0.0000258"
|
||||
ixz="0.0000009"
|
||||
iyy="0.0002355"
|
||||
iyz="-0.0001213"
|
||||
izz="0.0036543" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_arm2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/left_arm2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.020127 0.0040341 0.17817"
|
||||
rpy="-1.6062 0.0074682 0" />
|
||||
<parent
|
||||
link="left_arm1_link" />
|
||||
<child
|
||||
link="left_arm2_link" />
|
||||
<axis
|
||||
xyz="-0.99997 0.0074634 0.0002647" />
|
||||
<limit
|
||||
lower="-1.5"
|
||||
upper="1.5"
|
||||
effort="18"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_shoulder_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0144831349305154 -0.000175271563297497 0.0209530786562937"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0131461" />
|
||||
<inertia
|
||||
ixx="0.0000070"
|
||||
ixy="0"
|
||||
ixz="0.0000017"
|
||||
iyy="0.0000071"
|
||||
iyz="0"
|
||||
izz="0.0000045" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_shoulder_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_shoulder_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_shoulder_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.06 0.0878 0.36288"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="right_shoulder_link" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-1.57"
|
||||
upper="1.57"
|
||||
effort="100"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm1_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00258654183724675 0.00163071193731268 -0.0247948407356193"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.5854363" />
|
||||
<inertia
|
||||
ixx="0.0008778"
|
||||
ixy="0.0000592"
|
||||
ixz="-0.0000064"
|
||||
iyy="0.0002836"
|
||||
iyz="-0.0000262"
|
||||
izz="0.0009288" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_arm1_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_arm1_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm1_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.0385"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="right_shoulder_link" />
|
||||
<child
|
||||
link="right_arm1_link" />
|
||||
<axis
|
||||
xyz="1 0 0" />
|
||||
<limit
|
||||
lower="-1.5"
|
||||
upper="1.5"
|
||||
effort="42.8"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm2_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.7955302" />
|
||||
<inertia
|
||||
ixx="0.0035728"
|
||||
ixy="0.0000412"
|
||||
ixz="-0.0000004"
|
||||
iyy="0.0002317"
|
||||
iyz="0.0000292"
|
||||
izz="0.0036666" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_arm2_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="/home/mrji/mujoco_learning/robot3/meshes/right_arm2_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm2_joint"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.017969 0.0026972 -0.17843"
|
||||
rpy="-1.5793 0 0" />
|
||||
<parent
|
||||
link="right_arm1_link" />
|
||||
<child
|
||||
link="right_arm2_link" />
|
||||
<axis
|
||||
xyz="-0.99993 -0.0121 -0.00010316" />
|
||||
<limit
|
||||
lower="-1.5"
|
||||
upper="1.5"
|
||||
effort="18"
|
||||
velocity="50" />
|
||||
</joint>
|
||||
</robot>
|
||||
264
RobotOS1/urdf/robot3.xml
Normal file
264
RobotOS1/urdf/robot3.xml
Normal file
@@ -0,0 +1,264 @@
|
||||
<mujoco model="robot3">
|
||||
<default>
|
||||
<default class="robot">
|
||||
<default class="motor">
|
||||
<joint />
|
||||
<motor />
|
||||
</default>
|
||||
<default class="visual">
|
||||
<geom material="visualgeom" contype="0" conaffinity="0" group="2" />
|
||||
</default>
|
||||
<default class="collision">
|
||||
<geom material="collision_material" condim="3" contype="0" conaffinity="1" group="1" solref="0.005 1" friction="1 0.01 0.01" />
|
||||
<equality solimp="0.99 0.999 1e-05" solref="0.005 1" />
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<compiler angle="radian" />
|
||||
<option timestep="0.001" gravity="0 0 -9.81" wind="0 0 0" integrator="RK4"
|
||||
density="1.625"
|
||||
viscosity="1.8e-5" />
|
||||
|
||||
<visual>
|
||||
<global realtime="1" />
|
||||
<quality shadowsize="2048" numslices="28" offsamples="4" />
|
||||
<headlight diffuse="1 1 1" specular="0.5 0.5 0.5" active="1" />
|
||||
<rgba fog="1 0 0 1" haze="1 1 1 1" />
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<texture name="plane" type="2d" builtin="checker" rgb1=".1 .1 .1" rgb2=".9 .9 .9"
|
||||
width="512" height="512" mark="cross" markrgb=".8 .8 .8" />
|
||||
<material name="plane" reflectance="0.3" texture="plane" texrepeat="1 1" texuniform="true" />
|
||||
<material name="metal" rgba="1 1 1 1" />
|
||||
<material name="rubber" rgba="0.5 0.5 0.5 1" />
|
||||
<material name="default_material" rgba="0.7 0.7 0.7 1" />
|
||||
<material name="collision_material" rgba="1.0 0.28 0.1 0.9" />
|
||||
<mesh name="base_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/base_link.STL" />
|
||||
<mesh name="left_hip_pitch_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_hip_pitch_link.STL" />
|
||||
<mesh name="left_leg_pitch_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_leg_pitch_link.STL" />
|
||||
<mesh name="left_wheel2_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_wheel2_link.STL" />
|
||||
<mesh name="left_wheel1_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_wheel1_link.STL" />
|
||||
<mesh name="right_hip_pitch_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_hip_pitch_link.STL" />
|
||||
<mesh name="right_leg_pitch_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_leg_pitch_link.STL" />
|
||||
<mesh name="right_wheel2_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_wheel2_link.STL" />
|
||||
<mesh name="right_wheel1_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_wheel1_link.STL" />
|
||||
<mesh name="left_shoulder_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_shoulder_link.STL" />
|
||||
<mesh name="left_arm1_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_arm1_link.STL" />
|
||||
<mesh name="left_arm2_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/left_arm2_link.STL" />
|
||||
<mesh name="right_shoulder_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_shoulder_link.STL" />
|
||||
<mesh name="right_arm1_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_arm1_link.STL" />
|
||||
<mesh name="right_arm2_link.STL" file="/home/mrji/mujoco_learning/robot3/meshes/right_arm2_link.STL" />
|
||||
</asset>
|
||||
|
||||
<!-- <custom>
|
||||
<numeric name="wheel_radius" data="0.1" />
|
||||
<numeric name="wheel_base_y" data="0.3445" />
|
||||
</custom> -->
|
||||
|
||||
<worldbody>
|
||||
<geom name="floor" pos="0 0 0" size="0 0 .25" type="plane" material="plane"
|
||||
condim="3" friction="1.5 0.3 0.05"
|
||||
solimp="0.9 0.95 0.001" solref="0.02 1"/>
|
||||
<light directional="true" ambient=".3 .3 .3" pos="30 30 30" dir="0 -2 -1"
|
||||
diffuse=".5 .5 .5" specular=".5 .5 .5" />
|
||||
<body name="base_link" pos="0 0 0.65" quat="1 0 0 0" childclass="robot">
|
||||
<freejoint name="floating_base" />
|
||||
<camera name="c1" mode="fixed" pos="0 0 0.1" fovy="80" quat="-0.5 -0.3 0.3 0.5" />
|
||||
<camera name="c2" mode="fixed" pos="0 0 0.15" fovy="80" euler="1.57 -1.57 0" />
|
||||
<camera name="c3" mode="fixed" pos="0 0 0.4" fovy="90" quat="-0.5 -0.37 0.37 0.5" />
|
||||
<camera name="c4" mode="fixed" pos="-0.13 0 0.4" fovy="90" quat="0.5 -0.37 0.37 -0.5" />
|
||||
<site name="imu" />
|
||||
<geom name="base_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="base_link.STL" class="collision" />
|
||||
<geom name="base_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="base_link.STL" class="visual" />
|
||||
|
||||
<!-- 加入雷达 -->
|
||||
<!-- <body name="laser" pos="0 0 0.3">
|
||||
<geom type="sphere" size="0.01" rgba="0.2 0.2 0.2 1" />
|
||||
<body euler="0 -1.0472 0">
|
||||
<replicate count="25" euler="0 0.08726 0" sep="BBB">
|
||||
<body euler="0 0 -0.7854">
|
||||
<replicate count="25" euler="0 0 0.06545" sep="AAA" offset="0.0 0.0 0.0">
|
||||
<site name="rf" pos="0.02 0 0" zaxis="1 0 0" size="0.001 0.001 0.001"
|
||||
rgba="0.2 0.2 0.2 1" />
|
||||
</replicate>
|
||||
</body>
|
||||
</replicate>
|
||||
</body>
|
||||
</body>
|
||||
-->
|
||||
<body name="left_hip_pitch_link" pos="-0.06 -0.1358 -0.037269" quat="0.7070973820890846 -0.7070999794075672 -0.003384618178084856 -0.0033846306105044543">
|
||||
<joint name="left_hip_pitch_joint" type="hinge" ref="0.5236" class="motor" range="-1.57 1.57" axis="0 0 -1" />
|
||||
<inertial pos="3.93767161736252E-06 0.161490126813208 0.0158567790568734" quat="1.0 0.0 0.0 0.0" mass="0.6475263" diaginertia="0.0182516 0.0181748 0.0003913" />
|
||||
<geom name="left_hip_pitch_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_hip_pitch_link.STL" class="collision" />
|
||||
<geom name="left_hip_pitch_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_hip_pitch_link.STL" class="visual" />
|
||||
<body name="left_leg_pitch_link" pos="0 0 -0.0295" quat="0.9999818497424056 0.0 0.0 0.006024963548188557">
|
||||
<joint name="left_leg_pitch_joint" type="hinge" ref="1.0472" class="motor" range="-1.57 1.57" axis="0 0 1" />
|
||||
<inertial pos="-2.63464180747426E-05 0.197159145100908 0.000498203326348359" quat="1.0 0.0 0.0 0.0" mass="0.5126336" diaginertia="0.0135141 0.0135986 0.0002025" />
|
||||
<geom name="left_leg_pitch_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_leg_pitch_link.STL" class="collision" />
|
||||
<geom name="left_leg_pitch_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_leg_pitch_link.STL" class="visual" />
|
||||
<body name="left_wheel2_link" pos="0 0.4805 0.004" quat="-3.6732022875794935e-06 -0.9999992334237316 0.0012381996836028366 4.548161396802735e-09">
|
||||
<joint name="left_wheel2_joint" type="hinge" ref="0.0" class="motor" axis="0 0 1" />
|
||||
<inertial pos="4.1585564079967E-08 -5.47447156262759E-06 0.0187838263233595" quat="1.0 0.0 0.0 0.0" mass="0.745" diaginertia="0.0004716 0.0007945 0.0004716" />
|
||||
<geom name="left_wheel2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_wheel2_link.STL" class="collision" friction="0.7 0.2 0.1"/>
|
||||
<geom name="left_wheel2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="left_wheel2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_wheel1_link" pos="0 0.48 0.01" quat="0.9999885442520924 0.0 0.0 0.0047865817219583965">
|
||||
<joint name="left_wheel1_joint" type="hinge" ref="0.0" class="motor" axis="0 0 1" />
|
||||
<inertial pos="1.97042367054093E-06 5.10773910927798E-06 0.0187838263233595" quat="1.0 0.0 0.0 0.0" mass="0.745" diaginertia="0.0004716 0.0007945 0.0004716" />
|
||||
<geom name="left_wheel1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_wheel1_link.STL" class="collision" friction="0.7 0.2 0.1" />
|
||||
<geom name="left_wheel1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="left_wheel1_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_hip_pitch_link" pos="-0.06 0.1358 -0.037269" quat="0.7071047344942819 0.7071073318397715 -0.0010285199169106727 0.001028523694882219">
|
||||
<joint name="right_hip_pitch_joint" type="hinge" ref="0.5236" class="motor" range="-1.57 1.57" axis="0 0 -1" />
|
||||
<inertial pos="-3.97209815898069E-06 -0.161490067811833 0.0158567790568734" quat="1.0 0.0 0.0 0.0" mass="0.6475263" diaginertia="0.0182531 0.0181748 0.0003897" />
|
||||
<geom name="right_hip_pitch_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_hip_pitch_link.STL" class="collision" />
|
||||
<geom name="right_hip_pitch_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_hip_pitch_link.STL" class="visual" />
|
||||
<body name="right_leg_pitch_link" pos="0 0 -0.0295" quat="0.9999989421423353 0.0 0.0 -0.001454549487097675">
|
||||
<joint name="right_leg_pitch_joint" type="hinge" ref="1.0472" class="motor" range="-1.57 1.57" axis="0 0 1" />
|
||||
<inertial pos="3.43008600186093E-05 -0.197159143801203 0.000498203308628542" quat="1.0 0.0 0.0 0.0" mass="0.5126336" diaginertia="0.0135142 0.0135986 0.0002024" />
|
||||
<geom name="right_leg_pitch_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_leg_pitch_link.STL" class="collision" />
|
||||
<geom name="right_leg_pitch_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_leg_pitch_link.STL" class="visual" />
|
||||
<body name="right_wheel2_link" pos="1.9386E-05 -0.4805 0.004" quat="-3.673205103346574e-06 0.9999999999932537 0.0 -0.0">
|
||||
<joint name="right_wheel2_joint" type="hinge" ref="0.0" class="motor" axis="0 0 1" />
|
||||
<inertial pos="-5.49216228909084E-08 5.47435401360152E-06 0.0187838263233596" quat="1.0 0.0 0.0 0.0" mass="0.745" diaginertia="0.0004716 0.0007945 0.0004716" />
|
||||
<geom name="right_wheel2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_wheel2_link.STL" class="collision" friction="0.7 0.2 0.1"/>
|
||||
<geom name="right_wheel2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="right_wheel2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_wheel1_link" pos="0 -0.48 0.01" quat="0.9999989421423353 0.0 0.0 -0.001454549487097675">
|
||||
<joint name="right_wheel1_joint" type="hinge" ref="0.0" class="motor" axis="0 0 1" />
|
||||
<inertial pos="-1.90651569380871E-06 -5.13193590762073E-06 0.0187838263233596" quat="1.0 0.0 0.0 0.0" mass="0.745" diaginertia="0.0004716 0.0007945 0.0004716" />
|
||||
<geom name="right_wheel1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_wheel1_link.STL" class="collision" friction="0.7 0.2 0.1" />
|
||||
<geom name="right_wheel1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="rubber" type="mesh" mesh="right_wheel1_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_shoulder_link" pos="-0.06 -0.0878 0.36288" quat="1.0 0.0 0.0 0.0">
|
||||
<joint name="left_shoulder_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0 0 1" damping="1" />
|
||||
<inertial pos="-0.0144837915216556 0.000108169744531657 0.0209530786562939" quat="1.0 0.0 0.0 0.0" mass="0.0131461" diaginertia="7e-06 7.1e-06 4.5e-06" />
|
||||
<geom name="left_shoulder_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_shoulder_link.STL" class="collision" />
|
||||
<geom name="left_shoulder_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_shoulder_link.STL" class="visual" />
|
||||
<body name="left_arm1_link" pos="0 0 0.0385" quat="0.7071005527533233 0.7071031500834525 -0.002640406144908195 -0.0026403964461726814">
|
||||
<joint name="left_arm1_joint" type="hinge" ref="0.0" class="motor" range="-1.3 2.3" axis="-1 0 0" damping="1" />
|
||||
<inertial pos="-0.00228631297282665 -0.000694770956028379 0.0248681244469774" quat="1.0 0.0 0.0 0.0" mass="0.5854363" diaginertia="0.0008783 0.0002821 0.0009299" />
|
||||
<geom name="left_arm1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_arm1_link.STL" class="collision" />
|
||||
<geom name="left_arm1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_arm1_link.STL" class="visual" />
|
||||
<body name="left_arm2_link" pos="0.020127 0.0040341 0.17817" quat="0.6944747196208786 -0.7195074150900038 0.002593250103544229 0.002686725126152307">
|
||||
<joint name="left_arm2_joint" type="hinge" ref="0.0" class="motor" range="-1 1" axis="-0.99997 0.0074634 0.0002647" damping="1" />
|
||||
<inertial pos="-0.0237485342372672 -0.0546317049801525 1.0877545797372E-05" quat="1.0 0.0 0.0 0.0" mass="0.7955302" diaginertia="0.0035648 0.0002355 0.0036543" />
|
||||
<geom name="left_arm2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="left_arm2_link.STL" class="collision" />
|
||||
<geom name="left_arm2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="left_arm2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_shoulder_link" pos="-0.06 0.0878 0.36288" quat="1.0 0.0 0.0 0.0">
|
||||
<joint name="right_shoulder_joint" type="hinge" ref="0.0" class="motor" range="-1.57 1.57" axis="0 0 1" damping="1" />
|
||||
<inertial pos="-0.0144831349305154 -0.000175271563297497 0.0209530786562937" quat="1.0 0.0 0.0 0.0" mass="0.0131461" diaginertia="7e-06 7.1e-06 4.5e-06" />
|
||||
<geom name="right_shoulder_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_shoulder_link.STL" class="collision" />
|
||||
<geom name="right_shoulder_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_shoulder_link.STL" class="visual" />
|
||||
<body name="right_arm1_link" pos="0 0 0.0385" quat="0.7071054825112363 0.7071080798594735 0.0 0.0">
|
||||
<joint name="right_arm1_joint" type="hinge" ref="0.0" class="motor" range="-1.3 2.3" axis="1 0 0" damping="1" />
|
||||
<inertial pos="-0.00258654183724675 0.00163071193731268 -0.0247948407356193" quat="1.0 0.0 0.0 0.0" mass="0.5854363" diaginertia="0.0008778 0.0002836 0.0009288" />
|
||||
<geom name="right_arm1_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_arm1_link.STL" class="collision" />
|
||||
<geom name="right_arm1_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_arm1_link.STL" class="visual" />
|
||||
<body name="right_arm2_link" pos="0.017969 0.0026972 -0.17843" quat="0.704093896181981 -0.7101068830530216 0.0 0.0">
|
||||
<joint name="right_arm2_joint" type="hinge" ref="0.0" class="motor" range="-1 1" axis="-0.99993 -0.0121 -0.00010316" damping="1" />
|
||||
<inertial pos="-0.0240175529876588 0.0558375631647781 2.11385309717427E-06" quat="1.0 0.0 0.0 0.0" mass="0.7955302" diaginertia="0.0035728 0.0002317 0.0036666" />
|
||||
<geom name="right_arm2_link_collision" pos="0 0 0" quat="1.0 0.0 0.0 0.0" type="mesh" mesh="right_arm2_link.STL" class="collision" />
|
||||
<geom name="right_arm2_link_visual" pos="0 0 0" quat="1.0 0.0 0.0 0.0" material="metal" type="mesh" mesh="right_arm2_link.STL" class="visual" />
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<site name="base_link_site" pos="0 0 0" quat="1 0 0 0" />
|
||||
<camera name="front_camera" mode="track" fovy="90.0" quat="4.329780281177467e-17 4.329780281177466e-17 0.7071067811865475 0.7071067811865476" pos="0.0 2.0 0.5" />
|
||||
<camera name="side_camera" mode="track" fovy="90.0" quat="-0.5 -0.4999999999999999 0.5 0.5000000000000001" pos="-2.0 0.0 0.5" />
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<motor name="left_wheel2_joint_ctrl" joint="left_wheel2_joint" class="motor" forcerange="-30 30" />
|
||||
<motor name="left_wheel1_joint_ctrl" joint="left_wheel1_joint" class="motor" forcerange="-30 30" />
|
||||
<motor name="right_wheel2_joint_ctrl" joint="right_wheel2_joint" class="motor" forcerange="-30 30" />
|
||||
<motor name="right_wheel1_joint_ctrl" joint="right_wheel1_joint" class="motor" forcerange="-30 30" />
|
||||
<motor name="left_shoulder_joint_ctrl" joint="left_shoulder_joint" class="motor" forcerange="-42.8 42.8" />
|
||||
<motor name="left_arm1_joint_ctrl" joint="left_arm1_joint" class="motor" forcerange="-42.8 42.8" />
|
||||
<motor name="left_arm2_joint_ctrl" joint="left_arm2_joint" class="motor" forcerange="-18 18" />
|
||||
<motor name="right_shoulder_joint_ctrl" joint="right_shoulder_joint" class="motor" forcerange="-42.8 42.8" />
|
||||
<motor name="right_arm1_joint_ctrl" joint="right_arm1_joint" class="motor" forcerange="-42.8 42.8" />
|
||||
<motor name="right_arm2_joint_ctrl" joint="right_arm2_joint" class="motor" forcerange="-18 18" />
|
||||
|
||||
<!-- 轮毂电机 (使用general类型实现速度+前馈控制) -->
|
||||
<!-- <general name="left_wheel2_vel" joint="left_wheel2_joint"
|
||||
dyntype="integrator" biastype="affine"
|
||||
gainprm="0.5 0 0 0 0 0 0 0 0 0"
|
||||
biasprm="0 0 0 -0.1 0 0 0 0 0 0"
|
||||
dynprm="0.001"
|
||||
forcerange="-30 30"/>
|
||||
|
||||
<general name="left_wheel1_vel" joint="left_wheel1_joint"
|
||||
dyntype="integrator" biastype="affine"
|
||||
gainprm="0.5 0 0 0 0 0 0 0 0 0"
|
||||
biasprm="0 0 0 -0.1 0 0 0 0 0 0"
|
||||
dynprm="0.001"
|
||||
forcerange="-30 30"/>
|
||||
|
||||
<general name="right_wheel2_vel" joint="right_wheel2_joint"
|
||||
dyntype="integrator" biastype="affine"
|
||||
gainprm="0.5 0 0 0 0 0 0 0 0 0"
|
||||
biasprm="0 0 0 -0.1 0 0 0 0 0 0"
|
||||
dynprm="0.001"
|
||||
forcerange="-30 30"/>
|
||||
|
||||
<general name="right_wheel1_vel" joint="right_wheel1_joint"
|
||||
dyntype="integrator" biastype="affine"
|
||||
gainprm="0.5 0 0 0 0 0 0 0 0 0"
|
||||
biasprm="0 0 0 -0.1 0 0 0 0 0 0"
|
||||
dynprm="0.001"
|
||||
forcerange="-30 30"/> -->
|
||||
|
||||
<!-- <velocity name="left_wheel2_vel" joint="left_wheel2_joint" kv="15" forcerange="-16.2 16.2"/>
|
||||
<velocity name="left_wheel1_vel" joint="left_wheel1_joint" kv="15" forcerange="-0.1 0.1"/>
|
||||
<velocity name="right_wheel2_vel" joint="right_wheel2_joint" kv="15" forcerange="-16.2 16.2"/>
|
||||
<velocity name="right_wheel1_vel" joint="right_wheel1_joint" kv="15" forcerange="-0.1 0.1"/> -->
|
||||
|
||||
<!-- 腰部电机 (低刚度位置模式) -->
|
||||
<position name="1_pos" joint="left_hip_pitch_joint" kp="500" kv="22"/>
|
||||
<position name="2_pos" joint="left_leg_pitch_joint" kp="500" kv="22"/>
|
||||
<position name="3_pos" joint="right_hip_pitch_joint" kp="500" kv="22"/>
|
||||
<position name="4_pos" joint="right_leg_pitch_joint" kp="500" kv="22"/>
|
||||
</actuator>
|
||||
|
||||
<contact>
|
||||
<exclude body1="base_link" body2="left_hip_pitch_link" />
|
||||
<exclude body1="left_hip_pitch_link" body2="left_leg_pitch_link" />
|
||||
<exclude body1="left_leg_pitch_link" body2="left_wheel2_link" />
|
||||
<exclude body1="left_hip_pitch_link" body2="left_wheel1_link" />
|
||||
<exclude body1="base_link" body2="right_hip_pitch_link" />
|
||||
<exclude body1="right_hip_pitch_link" body2="right_leg_pitch_link" />
|
||||
<exclude body1="right_leg_pitch_link" body2="right_wheel2_link" />
|
||||
<exclude body1="right_hip_pitch_link" body2="right_wheel1_link" />
|
||||
<exclude body1="base_link" body2="left_shoulder_link" />
|
||||
<exclude body1="left_shoulder_link" body2="left_arm1_link" />
|
||||
<exclude body1="left_arm1_link" body2="left_arm2_link" />
|
||||
<exclude body1="base_link" body2="right_shoulder_link" />
|
||||
<exclude body1="right_shoulder_link" body2="right_arm1_link" />
|
||||
<exclude body1="right_arm1_link" body2="right_arm2_link" />
|
||||
<exclude body1="left_leg_pitch_link" body2="base_link"/>
|
||||
<exclude body1="right_leg_pitch_link" body2="base_link"/>
|
||||
</contact>
|
||||
|
||||
<sensor>
|
||||
<framequat name='orientation' objtype='site' objname='imu' />
|
||||
<gyro name='base_ang_vel' site='imu' />
|
||||
<accelerometer name="accel" site="imu" />
|
||||
<!-- <rangefinder site="rf" /> -->
|
||||
<!-- <framepos name="base_link_site_pos" objtype="site" objname="base_link_site" />
|
||||
<framequat name="base_link_site_quat" objtype="site" objname="base_link_site" />
|
||||
<framelinvel name="base_link_site_linvel" objtype="site" objname="base_link_site" />
|
||||
<frameangvel name="base_link_site_angvel" objtype="site" objname="base_link_site" />
|
||||
<velocimeter name="base_link_site_vel" site="base_link_site" /> -->
|
||||
</sensor>
|
||||
</mujoco>
|
||||
108
RobotOS1/urdf/robot_pitch.py
Normal file
108
RobotOS1/urdf/robot_pitch.py
Normal file
@@ -0,0 +1,108 @@
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
# 期望的俯仰角度（弧度）
|
||||
x = 60 # 腿部岔开角度
|
||||
a = 90 - (x / 2)
|
||||
DESIRED_DEGREE = 10 # 输入一个角度,正值为俯，负值为仰
|
||||
DESIRED_PITCH = np.radians(DESIRED_DEGREE) * (a / 90) # 转换为弧度并调整比例
|
||||
|
||||
def main():
|
||||
# 加载模型和创建模拟
|
||||
model = mujoco.MjModel.from_xml_path("/home/mrji/mujoco_learning/robot3/urdf/robot3.xml")
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
# 完整重置机器人状态
|
||||
mujoco.mj_resetData(model, data) # 确保所有状态完全重置
|
||||
|
||||
# 初始姿态设置 - 确保设置所有相关关节
|
||||
data.qpos[model.joint("left_hip_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("right_hip_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("left_leg_pitch_joint").id] = 0
|
||||
data.qpos[model.joint("right_leg_pitch_joint").id] = 0
|
||||
|
||||
# 重置所有速度为0
|
||||
data.qvel[:] = 0
|
||||
|
||||
# 修改：在最开始的时候额外重置一次
|
||||
mujoco.mj_resetData(model, data)
|
||||
|
||||
# 创建查看器
|
||||
with mujoco.viewer.launch_passive(model, data) as viewer:
|
||||
# 设置摄像头位置
|
||||
viewer.cam.azimuth = 180 # 面向机器人正面
|
||||
viewer.cam.elevation = -20 # 稍微俯视
|
||||
viewer.cam.distance = 2.0 # 距离
|
||||
viewer.cam.lookat[:] = [0.0, 0.0, 0.65] # 看向机器人中心
|
||||
|
||||
# 获取执行器ID
|
||||
hip_left = model.actuator("1_pos").id
|
||||
leg_left = model.actuator("2_pos").id
|
||||
hip_right = model.actuator("3_pos").id
|
||||
leg_right = model.actuator("4_pos").id
|
||||
|
||||
# 初始化控制信号
|
||||
data.ctrl[hip_left] = 0
|
||||
data.ctrl[leg_left] = 0
|
||||
data.ctrl[hip_right] = 0
|
||||
data.ctrl[leg_right] = 0
|
||||
|
||||
# 状态控制变量
|
||||
start_time = time.time()
|
||||
current_phase = "STABLE" # STABLE, MOVING, DONE
|
||||
transition_start_time = 0
|
||||
transition_duration = 1.0 # 过渡时间为1秒
|
||||
current_hip_target = 0 # 当前的目标角度值
|
||||
|
||||
# 初始稳定步骤 - 让物理引擎稳定
|
||||
for _ in range(100):
|
||||
mujoco.mj_step(model, data)
|
||||
viewer.sync()
|
||||
time.sleep(0.001)
|
||||
|
||||
# 模拟循环
|
||||
while viewer.is_running():
|
||||
step_start = time.time()
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
# 状态机控制
|
||||
if current_phase == "STABLE":
|
||||
# 稳定阶段 - 保持初始姿态1秒
|
||||
if elapsed_time >= 1.0:
|
||||
current_phase = "MOVING"
|
||||
transition_start_time = time.time()
|
||||
print("开始向目标角度过渡...")
|
||||
|
||||
elif current_phase == "MOVING":
|
||||
# 过渡阶段 - 在1秒内平滑过渡到目标角度
|
||||
transition_elapsed = time.time() - transition_start_time
|
||||
if transition_elapsed >= transition_duration:
|
||||
current_hip_target = DESIRED_PITCH
|
||||
current_phase = "DONE"
|
||||
print(f"达到目标俯仰角度: {DESIRED_DEGREE}度")
|
||||
else:
|
||||
# 线性插值过渡
|
||||
fraction = transition_elapsed / transition_duration
|
||||
current_hip_target = DESIRED_PITCH * fraction
|
||||
|
||||
# 更新控制信号
|
||||
data.ctrl[hip_left] = current_hip_target
|
||||
data.ctrl[hip_right] = -current_hip_target
|
||||
data.ctrl[leg_left] = 0
|
||||
data.ctrl[leg_right] = 0
|
||||
|
||||
# 执行物理模拟
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
# 同步查看器
|
||||
viewer.sync()
|
||||
|
||||
# 控制步长（约1000Hz）
|
||||
time_until_next_step = 0.001 - (time.time() - step_start)
|
||||
if time_until_next_step > 0:
|
||||
time.sleep(time_until_next_step)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
133
RobotOS1/urdf/self_rotation.py
Normal file
133
RobotOS1/urdf/self_rotation.py
Normal file
@@ -0,0 +1,133 @@
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import time
|
||||
from mujoco import viewer
|
||||
|
||||
def main():
|
||||
# 加载模型
|
||||
model = mujoco.MjModel.from_xml_path('/home/mrji/mujoco_learning/robot3/urdf/robot3.xml')
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
# 获取执行器索引
|
||||
actuator_names = [
|
||||
"left_wheel2_vel", "left_wheel1_vel",
|
||||
"right_wheel2_vel", "right_wheel1_vel"
|
||||
]
|
||||
actuator_ids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) for name in actuator_names]
|
||||
|
||||
# 设置自转参数 (8秒一圈)
|
||||
rotation_time = 2.2 # 可调范围5-10秒
|
||||
|
||||
# 机器人结构参数（需要根据实际模型调整）
|
||||
effective_wheel_radius = 0.12 # 轮子有效半径（米）
|
||||
track_width = 0.7 # 左右轮中心距（米）
|
||||
|
||||
# 计算轮子角速度差
|
||||
angular_velocity = 2 * np.pi / rotation_time # 机器人旋转角速度(rad/s)
|
||||
wheel_angular_difference = angular_velocity * track_width / (2 * effective_wheel_radius)
|
||||
|
||||
print(f"目标旋转时间: {rotation_time}秒/圈")
|
||||
print(f"轮子角速度差: {wheel_angular_difference:.2f} rad/s")
|
||||
|
||||
# 使用内置查看器
|
||||
with viewer.launch_passive(model, data) as viewer_handle:
|
||||
start_time = time.time()
|
||||
rotation_count = 0
|
||||
last_yaw = 0
|
||||
last_print_time = start_time
|
||||
|
||||
print("开始模拟...按 Ctrl+C 停止")
|
||||
|
||||
# 设置相机
|
||||
viewer_handle.cam.distance = 3.0
|
||||
viewer_handle.cam.elevation = -20
|
||||
viewer_handle.cam.azimuth = 45
|
||||
|
||||
# 初始稳定阶段（1秒）
|
||||
init_stable_time = 0.5
|
||||
init_end_time = start_time + init_stable_time
|
||||
|
||||
# 渐进加速参数
|
||||
acceleration_duration = 10.0 # 加速时间
|
||||
current_speed_factor = 0.0
|
||||
max_speed_factor = 1.0
|
||||
|
||||
# 运行模拟
|
||||
while viewer_handle.is_running():
|
||||
current_time = time.time()
|
||||
elapsed_time = current_time - start_time
|
||||
|
||||
# 初始稳定阶段
|
||||
if current_time < init_end_time:
|
||||
# 保持静止，让机器人稳定
|
||||
data.ctrl[actuator_ids] = [0, 0, 0, 0]
|
||||
else:
|
||||
# 渐进加速
|
||||
if current_speed_factor < max_speed_factor:
|
||||
current_speed_factor = min(max_speed_factor,
|
||||
(current_time - init_end_time) / acceleration_duration)
|
||||
|
||||
# 应用速度控制
|
||||
left_speed = current_speed_factor * wheel_angular_difference
|
||||
right_speed = -current_speed_factor * wheel_angular_difference
|
||||
|
||||
data.ctrl[actuator_ids[0]] = left_speed # left_wheel2
|
||||
data.ctrl[actuator_ids[1]] = left_speed # left_wheel1
|
||||
data.ctrl[actuator_ids[2]] = right_speed # right_wheel2
|
||||
data.ctrl[actuator_ids[3]] = right_speed # right_wheel1
|
||||
|
||||
# 步进模拟
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
# 更新查看器
|
||||
viewer_handle.sync()
|
||||
|
||||
# 获取机器人方向（使用基座方向）
|
||||
base_quat = data.qpos[3:7] # 基座的四元数
|
||||
roll, pitch, yaw = quaternion_to_euler(base_quat)
|
||||
|
||||
# 检测完整旋转
|
||||
if last_yaw > 3.0 and yaw < -3.0: # 从正π到负π
|
||||
rotation_count += 1
|
||||
elif last_yaw < -3.0 and yaw > 3.0: # 从负π到正π
|
||||
rotation_count += 1
|
||||
last_yaw = yaw
|
||||
|
||||
# 每秒显示状态
|
||||
if current_time - last_print_time >= 1.0:
|
||||
if rotation_count > 0:
|
||||
actual_time_per_rotation = elapsed_time / rotation_count
|
||||
print(f"目标: {rotation_time:.1f}秒/圈 | 实际: {actual_time_per_rotation:.1f}秒/圈 | 旋转圈数: {rotation_count}")
|
||||
else:
|
||||
print(f"旋转中...当前偏航角: {yaw:.2f} rad | 速度因子: {current_speed_factor:.2f}")
|
||||
last_print_time = current_time
|
||||
|
||||
# 控制模拟速度
|
||||
time.sleep(model.opt.timestep)
|
||||
|
||||
print("模拟结束")
|
||||
|
||||
def quaternion_to_euler(q):
|
||||
"""将四元数转换为欧拉角 (roll, pitch, yaw)"""
|
||||
w, x, y, z = q
|
||||
# 绕x轴旋转 (roll)
|
||||
sinr_cosp = 2 * (w * x + y * z)
|
||||
cosr_cosp = 1 - 2 * (x * x + y * y)
|
||||
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# 绕y轴旋转 (pitch)
|
||||
sinp = 2 * (w * y - z * x)
|
||||
if np.abs(sinp) >= 1:
|
||||
pitch = np.copysign(np.pi / 2, sinp)
|
||||
else:
|
||||
pitch = np.arcsin(sinp)
|
||||
|
||||
# 绕z轴旋转 (yaw)
|
||||
siny_cosp = 2 * (w * z + x * y)
|
||||
cosy_cosp = 1 - 2 * (y * y + z * z)
|
||||
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return roll, pitch, yaw
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
183
RobotOS1/urdf/self_rotation2.py
Normal file
183
RobotOS1/urdf/self_rotation2.py
Normal file
@@ -0,0 +1,183 @@
|
||||
import mujoco
|
||||
import numpy as np
|
||||
import time
|
||||
from mujoco import viewer
|
||||
|
||||
def main():
|
||||
# 加载模型
|
||||
model = mujoco.MjModel.from_xml_path('/home/mrji/mujoco_learning/robot3/urdf/robot3.xml')
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
# 获取执行器索引
|
||||
actuator_names = [
|
||||
"left_wheel2_vel", "left_wheel1_vel",
|
||||
"right_wheel2_vel", "right_wheel1_vel"
|
||||
]
|
||||
actuator_ids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) for name in actuator_names]
|
||||
|
||||
# 设置自转参数 (8秒一圈)
|
||||
rotation_time = 10.0 # 可调范围5-10秒
|
||||
|
||||
# 机器人结构参数（需要根据实际模型调整）
|
||||
effective_wheel_radius = 0.12 # 轮子有效半径（米）
|
||||
track_width = 0.7 # 左右轮中心距（米）
|
||||
|
||||
# 计算轮子角速度差
|
||||
angular_velocity = 2 * np.pi / rotation_time # 机器人旋转角速度(rad/s)
|
||||
wheel_angular_difference = angular_velocity * track_width / (2 * effective_wheel_radius)
|
||||
|
||||
print(f"目标旋转时间: {rotation_time}秒/圈")
|
||||
print(f"轮子角速度差: {wheel_angular_difference:.2f} rad/s")
|
||||
|
||||
# 使用内置查看器
|
||||
with viewer.launch_passive(model, data) as viewer_handle:
|
||||
start_time = time.time()
|
||||
rotation_count = 0
|
||||
last_yaw = 0
|
||||
last_print_time = start_time
|
||||
|
||||
print("开始模拟...按 Ctrl+C 停止")
|
||||
|
||||
# 设置相机
|
||||
viewer_handle.cam.distance = 3.0
|
||||
viewer_handle.cam.elevation = -20
|
||||
viewer_handle.cam.azimuth = 45
|
||||
|
||||
# 初始稳定阶段（1秒）
|
||||
init_stable_time = 0.5
|
||||
init_end_time = start_time + init_stable_time
|
||||
|
||||
# 渐进加速参数
|
||||
acceleration_duration = 10.0 # 加速时间
|
||||
current_speed_factor = 0.0
|
||||
max_speed_factor = 1.0
|
||||
|
||||
# 运行模拟
|
||||
while viewer_handle.is_running():
|
||||
current_time = time.time()
|
||||
elapsed_time = current_time - start_time
|
||||
|
||||
# 初始稳定阶段
|
||||
if current_time < init_end_time:
|
||||
# 保持静止，让机器人稳定
|
||||
data.ctrl[actuator_ids] = [0, 0, 0, 0]
|
||||
else:
|
||||
# 渐进加速
|
||||
if current_speed_factor < max_speed_factor:
|
||||
current_speed_factor = min(max_speed_factor,
|
||||
(current_time - init_end_time) / acceleration_duration)
|
||||
|
||||
# 应用速度控制
|
||||
left_speed = current_speed_factor * wheel_angular_difference
|
||||
right_speed = -current_speed_factor * wheel_angular_difference
|
||||
|
||||
# 方法1：预分配固定长度列表
|
||||
cur_dq = [0.0] * 4 # [0.0, 0.0, 0.0, 0.0]
|
||||
des_dq = [0.0] * 4
|
||||
tau = [0.0] * 4
|
||||
# cur_dq[0] = data.qvel[0]#
|
||||
# cur_dq[1] = data.qvel[1]#cur_dq[0] = data.qvel[0]#
|
||||
# cur_dq[2] = data.qvel[2]#
|
||||
# cur_dq[3] = data.qvel[3]#
|
||||
# 获取关节在速度向量中的索引
|
||||
wheel_joint_names = ["left_wheel2_joint", "left_wheel1_joint",
|
||||
"right_wheel2_joint", "right_wheel1_joint"]
|
||||
cur_dq = [
|
||||
data.qvel[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)]
|
||||
for name in wheel_joint_names
|
||||
]
|
||||
kd = 1
|
||||
des_dq[0]= left_speed
|
||||
|
||||
|
||||
des_dq[1]= 0.85 * left_speed
|
||||
des_dq[2]= right_speed
|
||||
des_dq[3]= 0.85 * right_speed
|
||||
|
||||
|
||||
des_dq[0] = -1
|
||||
des_dq[1] = -1
|
||||
des_dq[2] = -1
|
||||
des_dq[3] = 1
|
||||
|
||||
tau[0] = kd *(des_dq[0]- cur_dq[0])
|
||||
tau[1] = kd *(des_dq[1]- cur_dq[1])
|
||||
tau[2] = kd *(des_dq[2]- cur_dq[2])
|
||||
tau[3] = kd *(des_dq[3]- cur_dq[3])
|
||||
|
||||
# data.ctrl[actuator_ids[0]] = left_speed # left_wheel2
|
||||
# data.ctrl[actuator_ids[1]] = 0.85 * left_speed # left_wheel1
|
||||
# data.ctrl[actuator_ids[2]] = right_speed # right_wheel2
|
||||
# data.ctrl[actuator_ids[3]] = 0.85 * right_speed # right_wheel1
|
||||
|
||||
# 获取执行器索引
|
||||
actuator_names1 = [
|
||||
"left_wheel2_joint_ctrl", "left_wheel1_joint_ctrl",
|
||||
"right_wheel2_joint_ctrl", "right_wheel1_joint_ctrl"
|
||||
]
|
||||
actuator_ids1 = [
|
||||
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
|
||||
for name in actuator_names1
|
||||
]
|
||||
data.ctrl[actuator_ids1[0]] = tau[0] # left_wheel2
|
||||
data.ctrl[actuator_ids1[1]] = tau[1] # left_wheel1
|
||||
data.ctrl[actuator_ids1[2]] = tau[2] # right_wheel2
|
||||
data.ctrl[actuator_ids1[3]] = tau[3] # right_wheel1
|
||||
|
||||
# 步进模拟
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
# 更新查看器
|
||||
viewer_handle.sync()
|
||||
|
||||
# 获取机器人方向（使用基座方向）
|
||||
base_quat = data.qpos[3:7] # 基座的四元数
|
||||
roll, pitch, yaw = quaternion_to_euler(base_quat)
|
||||
|
||||
# 检测完整旋转
|
||||
if last_yaw > 3.0 and yaw < -3.0: # 从正π到负π
|
||||
rotation_count += 1
|
||||
elif last_yaw < -3.0 and yaw > 3.0: # 从负π到正π
|
||||
rotation_count += 1
|
||||
last_yaw = yaw
|
||||
|
||||
# 每秒显示状态
|
||||
if current_time - last_print_time >= 1.0:
|
||||
if rotation_count > 0:
|
||||
actual_time_per_rotation = elapsed_time / rotation_count
|
||||
print(f"目标: {rotation_time:.1f}秒/圈 | 实际: {actual_time_per_rotation:.1f}秒/圈 | 旋转圈数: {rotation_count}")
|
||||
else:
|
||||
print(f"旋转中...当前偏航角: {yaw:.2f} rad | 速度因子: {current_speed_factor:.2f}")
|
||||
last_print_time = current_time
|
||||
|
||||
# 控制模拟速度
|
||||
time.sleep(0.001)
|
||||
#time.sleep(model.opt.timestep)
|
||||
|
||||
|
||||
print("模拟结束")
|
||||
|
||||
def quaternion_to_euler(q):
|
||||
"""将四元数转换为欧拉角 (roll, pitch, yaw)"""
|
||||
w, x, y, z = q
|
||||
# 绕x轴旋转 (roll)
|
||||
sinr_cosp = 2 * (w * x + y * z)
|
||||
cosr_cosp = 1 - 2 * (x * x + y * y)
|
||||
roll = np.arctan2(sinr_cosp, cosr_cosp)
|
||||
|
||||
# 绕y轴旋转 (pitch)
|
||||
sinp = 2 * (w * y - z * x)
|
||||
if np.abs(sinp) >= 1:
|
||||
pitch = np.copysign(np.pi / 2, sinp)
|
||||
else:
|
||||
pitch = np.arcsin(sinp)
|
||||
|
||||
# 绕z轴旋转 (yaw)
|
||||
siny_cosp = 2 * (w * z + x * y)
|
||||
cosy_cosp = 1 - 2 * (y * y + z * z)
|
||||
yaw = np.arctan2(siny_cosp, cosy_cosp)
|
||||
|
||||
return roll, pitch, yaw
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
43
RobotOS1/urdf/test_glfw.py
Normal file
43
RobotOS1/urdf/test_glfw.py
Normal file
@@ -0,0 +1,43 @@
|
||||
import glfw
|
||||
import OpenGL.GL as gl
|
||||
|
||||
def main():
|
||||
# 初始化 GLFW
|
||||
if not glfw.init():
|
||||
print("GLFW 初始化失败")
|
||||
return
|
||||
|
||||
print("GLFW 初始化成功")
|
||||
|
||||
# 创建窗口
|
||||
window = glfw.create_window(800, 600, "GLFW 窗口测试", None, None)
|
||||
if not window:
|
||||
print("无法创建 GLFW 窗口")
|
||||
glfw.terminate()
|
||||
return
|
||||
|
||||
print("GLFW 窗口创建成功")
|
||||
|
||||
# 设置当前上下文
|
||||
glfw.make_context_current(window)
|
||||
|
||||
# 设置背景颜色
|
||||
gl.glClearColor(0.2, 0.3, 0.6, 1.0) # RGBA - 深蓝色
|
||||
|
||||
# 主循环
|
||||
while not glfw.window_should_close(window):
|
||||
# 清除缓冲区
|
||||
gl.glClear(gl.GL_COLOR_BUFFER_BIT)
|
||||
|
||||
# 交换缓冲区
|
||||
glfw.swap_buffers(window)
|
||||
|
||||
# 处理事件
|
||||
glfw.poll_events()
|
||||
|
||||
# 清理
|
||||
glfw.terminate()
|
||||
print("GLFW 已终止")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user