add ik solver s w e config
This commit is contained in:
parent
8aa5908300
commit
5880a20fce
@ -21,6 +21,21 @@ find_package(gRPC REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
pkg_check_modules(GLOG REQUIRED libglog)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/pinocchio/3.8.0/include
|
||||
${CMAKE_SOURCE_DIR}/third_party/coal/3.0.2/include
|
||||
${CMAKE_SOURCE_DIR}/third_party/urdfdom/5.0.3/include
|
||||
${CMAKE_SOURCE_DIR}/third_party/urdfdom_headers/2.0.1/include
|
||||
${CMAKE_SOURCE_DIR}/third_party/mujoco/3.3.7/include
|
||||
)
|
||||
|
||||
link_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/pinocchio/3.8.0/lib
|
||||
${CMAKE_SOURCE_DIR}/third_party/coal/3.0.2/lib
|
||||
${CMAKE_SOURCE_DIR}/third_party/urdfdom/5.0.3/lib
|
||||
${CMAKE_SOURCE_DIR}/third_party/mujoco/3.3.7/lib
|
||||
)
|
||||
|
||||
############################################################
|
||||
# PROTO
|
||||
############################################################
|
||||
@ -58,7 +73,7 @@ include_directories(
|
||||
/usr/include/opencv4
|
||||
/usr/include/eigen3
|
||||
/usr/local/include/osqp
|
||||
/usr/local/share/osqp/codegen_files/inc/private/
|
||||
/usr/local/share/osqp/codegen_files/inc/prate/
|
||||
$ENV{HOME}/.local/include
|
||||
/usr/local/include/
|
||||
${PROTO_BINARY_DIR}
|
||||
@ -97,7 +112,7 @@ target_link_libraries(cmvr_es PRIVATE
|
||||
cmvr_es::hardware
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::device::ti5motor
|
||||
cmvr_es::ctrl::controller
|
||||
cmvr_es::controller
|
||||
cmvr_es::data_center
|
||||
cmvr_es::ik_solver
|
||||
cmvr_es::planner
|
||||
|
||||
@ -50,6 +50,8 @@ sudo apt install gstreamer1.0-plugins-ugly gstreamer1.0-libav -y
|
||||
|
||||
# tinyxml2
|
||||
sudo apt install libtinyxml2-dev -y
|
||||
|
||||
sudo pip install eigenpy
|
||||
```
|
||||
|
||||
### 3. gRPC install (build from source)
|
||||
|
||||
@ -20,9 +20,9 @@
|
||||
</Camera>
|
||||
|
||||
<DexHand>
|
||||
<!-- <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">-->
|
||||
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
|
||||
<!-- </RH56DFTP>-->
|
||||
<RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">
|
||||
<Freedom order="01" default_force="500" default_speed="500" />
|
||||
</RH56DFTP>
|
||||
<!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
|
||||
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
|
||||
<!-- </RH56DFTP>-->
|
||||
@ -40,22 +40,22 @@
|
||||
bufferSize="50"
|
||||
verbose="false">
|
||||
<CanManger id="" devId="">
|
||||
<LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
|
||||
<LeftArmCan id = " " devId = " " channelId ="0" enable="true" toolFrame="L_FINGER_TIP">
|
||||
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="21" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="22" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
</LeftArmCan>
|
||||
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
|
||||
<RightArmCan id = " " devId = " " channelId ="1" enable="false" toolFrame="R_FINGER_TIP">
|
||||
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
|
||||
<Motor id="28" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
|
||||
<Motor id="1" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
|
||||
</RightArmCan>
|
||||
<HeadCan id = " " devId = " " channelId ="2" enable="false">
|
||||
@ -64,7 +64,7 @@
|
||||
<Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
</HeadCan>
|
||||
<WaistCan id = " " devId = " " channelId ="3" enable="false">
|
||||
<Motor id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
</WaistCan>
|
||||
</CanManger>
|
||||
|
||||
9
config/ik_solver_config/pinocchio_dls_ik.pb.txt
Normal file
9
config/ik_solver_config/pinocchio_dls_ik.pb.txt
Normal file
@ -0,0 +1,9 @@
|
||||
urdf_path: "/home/lgv/cmvr/dual_arm.urdf"
|
||||
base_frame_name: "PELVIS_S"
|
||||
flange_frame_name: "R_FLANGE"
|
||||
tcp_frame_name: "R_TCP"
|
||||
|
||||
max_iters: 100
|
||||
pos_eps: 1e-6
|
||||
rot_eps: 1e-6
|
||||
damping: 1e-6
|
||||
10
config/ik_solver_config/pinocchio_qp_ik.pb.txt
Normal file
10
config/ik_solver_config/pinocchio_qp_ik.pb.txt
Normal file
@ -0,0 +1,10 @@
|
||||
urdf_path: "/home/lgv/cmvr/dual_arm.urdf"
|
||||
base_frame_name: "PELVIS_S"
|
||||
flange_frame_name: "R_FLANGE"
|
||||
tcp_frame_name: "R_TCP"
|
||||
|
||||
lambda: 0.0001
|
||||
w_posrot: 0.5
|
||||
max_iters: 80
|
||||
tol: 1e-6
|
||||
qp_time_limit: 0.005
|
||||
BIN
config/robot_description/hc_description.tar
Normal file
BIN
config/robot_description/hc_description.tar
Normal file
Binary file not shown.
@ -1,5 +1,52 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="dual_arm">
|
||||
<mujoco>
|
||||
<compiler
|
||||
meshdir="meshes"
|
||||
balanceinertia="true"
|
||||
discardvisual="false" />
|
||||
</mujoco>
|
||||
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.6" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="1.2"/>
|
||||
</geometry>
|
||||
<material name="gray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0.6" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="1.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="25.4469"/>
|
||||
|
||||
<inertia
|
||||
ixx="3.06953"
|
||||
ixy="0.0"
|
||||
ixz="0.0"
|
||||
iyy="3.06953"
|
||||
iyz="0.0"
|
||||
izz="0.03181"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 1.2"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="PELVIS_S"/>
|
||||
</joint>
|
||||
|
||||
<link name="PELVIS_S">
|
||||
<inertial>
|
||||
<origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
|
||||
@ -323,7 +370,7 @@
|
||||
<parent link="PELVIS_S" />
|
||||
<child link="R_SHOULDER_P_S" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
|
||||
<limit lower="-3.14" upper="3.14" effort="120" velocity="3.351" />
|
||||
</joint>
|
||||
|
||||
<link name="R_SHOULDER_R_S">
|
||||
@ -360,7 +407,7 @@
|
||||
<parent link="R_SHOULDER_P_S" />
|
||||
<child link="R_SHOULDER_R_S" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit lower="-2" upper="2" effort="120" velocity="3.351" />
|
||||
<limit lower="-0.78" upper="1.57" effort="120" velocity="3.351" />
|
||||
</joint>
|
||||
|
||||
<link name="R_SHOULDER_Y_S">
|
||||
@ -398,7 +445,7 @@
|
||||
<parent link="R_SHOULDER_R_S" />
|
||||
<child link="R_SHOULDER_Y_S" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
|
||||
<limit lower="-3.14" upper="3.14" effort="80" velocity="3.8758" />
|
||||
</joint>
|
||||
|
||||
<link name="R_ELBOW_R_S">
|
||||
@ -472,7 +519,7 @@
|
||||
<parent link="R_ELBOW_R_S" />
|
||||
<child link="R_WRIST_P_S" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
|
||||
<limit lower="-3.14" upper="3.14" effort="50" velocity="4.71" />
|
||||
</joint>
|
||||
|
||||
<link name="R_WRIST_Y_S">
|
||||
|
||||
206
config/robot_description/hc_description/dual_arm.xml
Normal file
206
config/robot_description/hc_description/dual_arm.xml
Normal file
@ -0,0 +1,206 @@
|
||||
<mujoco model="dual_arm">
|
||||
<compiler angle="radian" meshdir="meshes/"/>
|
||||
|
||||
<default>
|
||||
<joint damping="10" armature="0.001"/>
|
||||
</default>
|
||||
|
||||
<asset>
|
||||
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
|
||||
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
|
||||
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
|
||||
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
|
||||
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
|
||||
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
|
||||
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
|
||||
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
|
||||
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
|
||||
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
|
||||
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
|
||||
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
|
||||
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
|
||||
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
|
||||
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
|
||||
|
||||
<!-- 天空背景，可要可不要 -->
|
||||
<texture name="skybox" type="skybox"
|
||||
builtin="gradient"
|
||||
rgb1="0.4 0.6 0.8"
|
||||
rgb2="0 0 0"
|
||||
width="512" height="512"/>
|
||||
|
||||
<!-- 蓝色方格地面纹理：builtin="checker" -->
|
||||
<texture name="grid"
|
||||
type="2d"
|
||||
builtin="checker"
|
||||
width="512" height="512"
|
||||
rgb1="0.2 0.3 0.4"
|
||||
rgb2="0.1 0.2 0.3"
|
||||
mark="cross"
|
||||
markrgb="0.8 0.8 0.8"/>
|
||||
|
||||
<material name="grid_floor"
|
||||
texture="grid"
|
||||
texrepeat="5 5"
|
||||
rgba="1 1 1.0 1"
|
||||
emission="0.9"
|
||||
specular="0.5"
|
||||
shininess="1"
|
||||
reflectance="0.3"/>
|
||||
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom size="0.05 0.6" pos="0 0 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
|
||||
<geom size="0.05 0.6" pos="0 0 0.6" type="cylinder"/>
|
||||
<geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||||
<geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||||
<body name="L_SHOULDER_P_S" pos="0 0.0945 1.242">
|
||||
<inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||||
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-1.57 0.26" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
|
||||
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
|
||||
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
|
||||
<inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||||
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.78" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
|
||||
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
|
||||
<inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||||
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
|
||||
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
|
||||
<inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||||
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-2.05 0" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
|
||||
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
|
||||
<inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||||
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
|
||||
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
|
||||
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
|
||||
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||||
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
|
||||
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
|
||||
<inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
|
||||
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.26" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="R_SHOULDER_P_S" pos="0 -0.0945 1.242">
|
||||
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||||
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
|
||||
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
|
||||
<inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||||
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-0.78 1.57" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
|
||||
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
|
||||
<inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||||
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
|
||||
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
|
||||
<inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||||
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.05" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
|
||||
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
|
||||
<inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||||
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
|
||||
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
|
||||
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||||
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
|
||||
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
|
||||
<inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.504366" diaginertia="0.00027189 0.000186086 0.000130629"/>
|
||||
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
|
||||
<geom size="0.004" pos="0.00684256 -0.284077 0.00801525" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 1 1"/>
|
||||
<geom size="0.005" pos="0.00684256 -0.284077 0.00801525" rgba="0 1 1 1"/>
|
||||
<geom size="0.004" pos="-0.01212 -0.17655 0.07506" quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 0 1"/>
|
||||
<geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107" rgba="0 1 0 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<light name="top_light"
|
||||
mode="track"
|
||||
directional="true"
|
||||
diffuse="0.4 0.4 0.4"
|
||||
specular="0.3 0.3 0.3"
|
||||
pos="0 0 3"
|
||||
dir="0 0 -1"/>
|
||||
|
||||
<geom name="floor"
|
||||
type="plane"
|
||||
pos="0 0 0"
|
||||
size="0 0 0.05"
|
||||
material="grid_floor"
|
||||
condim="3"
|
||||
friction="1 0.005 0.0001"/>
|
||||
</worldbody>
|
||||
|
||||
|
||||
<!-- 关节空间 PD 位置控制：ctrl = 目标关节角度 (rad) -->
|
||||
<actuator>
|
||||
<!-- 左臂：位置控制 -->
|
||||
<position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
|
||||
kp="200" ctrlrange="-1.57 0.26"/>
|
||||
<position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
|
||||
kp="200" ctrlrange="-1.57 0.78"/>
|
||||
<position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
|
||||
kp="200" ctrlrange="-3.14 3.14"/>
|
||||
|
||||
<position name="L_ELBOW_R_pos" joint="L_ELBOW_R"
|
||||
kp="150" ctrlrange="-2.05 0.00"/>
|
||||
|
||||
<position name="L_WRIST_P_pos" joint="L_WRIST_P"
|
||||
kp="80" ctrlrange="-3.14 3.14"/>
|
||||
<position name="L_WRIST_Y_pos" joint="L_WRIST_Y"
|
||||
kp="80" ctrlrange="-0.78 0.78"/>
|
||||
<position name="L_WRIST_R_pos" joint="L_WRIST_R"
|
||||
kp="80" ctrlrange="-1.57 0.26"/>
|
||||
|
||||
<!-- 右臂：位置控制 -->
|
||||
<position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
|
||||
kp="2000" ctrlrange="-3.14 3.14"/>
|
||||
<position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
|
||||
kp="2000" ctrlrange="-0.78 1.57"/>
|
||||
<position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
|
||||
kp="2000" ctrlrange="-3.14 3.14"/>
|
||||
|
||||
<position name="R_ELBOW_R_pos" joint="R_ELBOW_R"
|
||||
kp="1500" ctrlrange="0 2.05"/>
|
||||
|
||||
<position name="R_WRIST_P_pos" joint="R_WRIST_P"
|
||||
kp="800" ctrlrange="-3.14 3.14"/>
|
||||
<position name="R_WRIST_Y_pos" joint="R_WRIST_Y"
|
||||
kp="800" ctrlrange="-0.78 0.78"/>
|
||||
<position name="R_WRIST_R_pos" joint="R_WRIST_R"
|
||||
kp="800" ctrlrange="-0.26 1.57"/>
|
||||
</actuator>
|
||||
|
||||
|
||||
|
||||
</mujoco>
|
||||
347
config/robot_description/hc_description/dual_arm_temp.xml
Normal file
347
config/robot_description/hc_description/dual_arm_temp.xml
Normal file
@ -0,0 +1,347 @@
|
||||
<mujoco model="dual_arm">
|
||||
<compiler angle="radian" meshdir="meshes/"/>
|
||||
|
||||
<!-- 全局默认：给所有关节一点阻尼和等效转子惯量 -->
|
||||
<default>
|
||||
<joint damping="10" armature="0.001"/>
|
||||
</default>
|
||||
|
||||
|
||||
|
||||
<!-- 稳定一点的时间步和积分器 -->
|
||||
<!-- <option timestep="0.001" integrator="RK4"/>-->
|
||||
|
||||
<asset>
|
||||
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
|
||||
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
|
||||
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
|
||||
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
|
||||
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
|
||||
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
|
||||
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
|
||||
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
|
||||
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
|
||||
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
|
||||
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
|
||||
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
|
||||
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
|
||||
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
|
||||
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
|
||||
|
||||
|
||||
<!-- 天空背景，可要可不要 -->
|
||||
<texture name="skybox" type="skybox"
|
||||
builtin="gradient"
|
||||
rgb1="0.4 0.6 0.8"
|
||||
rgb2="0 0 0"
|
||||
width="512" height="512"/>
|
||||
|
||||
<!-- 蓝色方格地面纹理：builtin="checker" -->
|
||||
<texture name="grid"
|
||||
type="2d"
|
||||
builtin="checker"
|
||||
width="512" height="512"
|
||||
rgb1="0.2 0.3 0.4"
|
||||
rgb2="0.1 0.2 0.3"
|
||||
mark="cross"
|
||||
markrgb="0.8 0.8 0.8"/>
|
||||
|
||||
<!-- 地面材质，绑定上面的纹理 -->
|
||||
<material name="grid_floor"
|
||||
texture="grid"
|
||||
texrepeat="5 5"
|
||||
reflectance="0.2"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||||
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||||
|
||||
<body name="L_SHOULDER_P_S" pos="0 0.0945 0.042">
|
||||
<inertial pos="-0.00982259 0.0704593 1.15262e-06"
|
||||
quat="0.706163 0.705933 -0.0386962 -0.0386741"
|
||||
mass="0.880738"
|
||||
diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||||
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0"
|
||||
range="-1.57 1.57" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
|
||||
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1"
|
||||
mesh="L_SHOULDER_P_S"/>
|
||||
|
||||
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
|
||||
<inertial pos="-0.0346025 0.0917393 -1.67085e-08"
|
||||
quat="0.358778 0.609303 -0.358823 0.609323"
|
||||
mass="0.594788"
|
||||
diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||||
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0"
|
||||
range="-2 2" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="L_SHOULDER_R_S"/>
|
||||
|
||||
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
|
||||
<inertial pos="-0.00440977 0.086362 9.50749e-09"
|
||||
quat="0.705001 0.704998 -0.054559 -0.0545441"
|
||||
mass="0.563406"
|
||||
diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||||
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0"
|
||||
range="-2.18 0" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="L_SHOULDER_Y_S"/>
|
||||
|
||||
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
|
||||
<inertial pos="-0.0335624 0.06032 2.99656e-07"
|
||||
quat="0.674756 0.674714 0.211333 0.211667"
|
||||
mass="0.393572"
|
||||
diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||||
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0"
|
||||
range="-2.05 0" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="L_ELBOW_R_S"/>
|
||||
|
||||
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
|
||||
<inertial pos="-1.39659e-10 0.0675973 0.0192006"
|
||||
quat="0.467537 0.530481 -0.530481 0.467537"
|
||||
mass="0.442332"
|
||||
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||||
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0"
|
||||
range="0 3.14" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
|
||||
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1"
|
||||
mesh="L_WRIST_P_S"/>
|
||||
|
||||
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
|
||||
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254"
|
||||
quat="0.298107 0.641196 0.641196 0.298107"
|
||||
mass="0.235738"
|
||||
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||||
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1"
|
||||
range="-0.78 0.78" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
|
||||
mesh="L_WRIST_Y_S"/>
|
||||
|
||||
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
|
||||
<inertial pos="-0.0161477 0.0955047 -0.00499392"
|
||||
quat="0.595488 0.369507 -0.591676 0.39847"
|
||||
mass="0.504894"
|
||||
diaginertia="0.000280921 0.000188575 0.000178055"/>
|
||||
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0"
|
||||
range="-1.57 0.26" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="L_WRIST_R_S"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
|
||||
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06"
|
||||
quat="0.706163 0.705933 0.0386962 0.0386741"
|
||||
mass="0.880738"
|
||||
diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||||
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0"
|
||||
range="-1.57 1.57" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="R_SHOULDER_P_S"/>
|
||||
|
||||
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
|
||||
<inertial pos="-0.0346025 -0.0917393 1.86281e-08"
|
||||
quat="0.609323 0.358823 -0.609303 0.358778"
|
||||
mass="0.594788"
|
||||
diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||||
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0"
|
||||
range="-2 2" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="R_SHOULDER_R_S"/>
|
||||
|
||||
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
|
||||
<inertial pos="-0.00440977 -0.086362 -7.58792e-09"
|
||||
quat="0.705001 0.704998 0.054559 0.0545441"
|
||||
mass="0.563406"
|
||||
diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||||
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0"
|
||||
range="0 3.14" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="R_SHOULDER_Y_S"/>
|
||||
|
||||
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
|
||||
<inertial pos="-0.0335624 -0.06032 -2.97736e-07"
|
||||
quat="0.674756 0.674714 -0.211333 -0.211667"
|
||||
mass="0.393572"
|
||||
diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||||
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0"
|
||||
range="0 2.05" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="R_ELBOW_R_S"/>
|
||||
|
||||
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
|
||||
<inertial pos="-1.39657e-10 -0.0675973 0.0192006"
|
||||
quat="0.530481 0.467537 -0.467537 0.530481"
|
||||
mass="0.442332"
|
||||
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||||
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0"
|
||||
range="-3.14 0" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
|
||||
mesh="R_WRIST_P_S"/>
|
||||
|
||||
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
|
||||
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254"
|
||||
quat="0.298107 0.641196 0.641196 0.298107"
|
||||
mass="0.235738"
|
||||
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||||
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1"
|
||||
range="-0.78 0.78" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
|
||||
mesh="R_WRIST_Y_S"/>
|
||||
|
||||
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
|
||||
<inertial pos="-0.0201642 -0.11075 -0.00598955"
|
||||
quat="0.483404 0.529786 -0.463203 0.520663"
|
||||
mass="0.504366"
|
||||
diaginertia="0.00027189 0.000186086 0.000130629"/>
|
||||
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0"
|
||||
range="-0.26 1.57" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
|
||||
rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
|
||||
mesh="R_WRIST_R_S"/>
|
||||
|
||||
<!-- 小标记点 -->
|
||||
<geom size="0.004"
|
||||
pos="0.00684256 -0.284077 0.00801525"
|
||||
contype="0" conaffinity="0" group="1"
|
||||
density="0" rgba="0 1 1 1"/>
|
||||
<geom size="0.005"
|
||||
pos="0.00684256 -0.284077 0.00801525"
|
||||
rgba="0 1 1 1"/>
|
||||
<geom size="0.004"
|
||||
pos="-0.01212 -0.17655 0.07506"
|
||||
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
|
||||
contype="0" conaffinity="0" group="1"
|
||||
density="0" rgba="0 1 0 1"/>
|
||||
<geom size="0.005"
|
||||
pos="-0.01212 -0.17655 0.07506"
|
||||
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
|
||||
rgba="0 1 0 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
|
||||
<light name="top_light"
|
||||
mode="track"
|
||||
directional="true"
|
||||
diffuse="1 1 1"
|
||||
specular="0.3 0.3 0.3"
|
||||
pos="0 0 3"
|
||||
dir="0 0 -1"/>
|
||||
|
||||
<geom name="floor"
|
||||
type="plane"
|
||||
pos="0 0 0"
|
||||
size="0 0 0.05"
|
||||
material="grid_floor"
|
||||
condim="3"
|
||||
friction="1 0.005 0.0001"/>
|
||||
</worldbody>
|
||||
|
||||
<!-- <actuator>-->
|
||||
<!-- <!– 左臂：力矩控制，ctrl = 关节力矩 (Nm) –>-->
|
||||
<!-- <motor name="L_SHOULDER_P_torque" joint="L_SHOULDER_P"-->
|
||||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||||
<!-- <motor name="L_SHOULDER_R_torque" joint="L_SHOULDER_R"-->
|
||||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||||
<!-- <motor name="L_SHOULDER_Y_torque" joint="L_SHOULDER_Y"-->
|
||||
<!-- gear="1" ctrlrange="-150 150"/>-->
|
||||
<!-- <motor name="L_ELBOW_R_torque" joint="L_ELBOW_R"-->
|
||||
<!-- gear="1" ctrlrange="-120 120"/>-->
|
||||
<!-- <motor name="L_WRIST_P_torque" joint="L_WRIST_P"-->
|
||||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||||
<!-- <motor name="L_WRIST_Y_torque" joint="L_WRIST_Y"-->
|
||||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||||
<!-- <motor name="L_WRIST_R_torque" joint="L_WRIST_R"-->
|
||||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||||
|
||||
<!-- <!– 右臂：力矩控制 –>-->
|
||||
<!-- <motor name="R_SHOULDER_P_torque" joint="R_SHOULDER_P"-->
|
||||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||||
<!-- <motor name="R_SHOULDER_R_torque" joint="R_SHOULDER_R"-->
|
||||
<!-- gear="1" ctrlrange="-200 200"/>-->
|
||||
<!-- <motor name="R_SHOULDER_Y_torque" joint="R_SHOULDER_Y"-->
|
||||
<!-- gear="1" ctrlrange="-150 150"/>-->
|
||||
<!-- <motor name="R_ELBOW_R_torque" joint="R_ELBOW_R"-->
|
||||
<!-- gear="1" ctrlrange="-120 120"/>-->
|
||||
<!-- <motor name="R_WRIST_P_torque" joint="R_WRIST_P"-->
|
||||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||||
<!-- <motor name="R_WRIST_Y_torque" joint="R_WRIST_Y"-->
|
||||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||||
<!-- <motor name="R_WRIST_R_torque" joint="R_WRIST_R"-->
|
||||
<!-- gear="1" ctrlrange="-80 80"/>-->
|
||||
<!-- </actuator>-->
|
||||
|
||||
<actuator>
|
||||
<!-- 左臂：位置控制，ctrl = 关节目标角度 (rad) -->
|
||||
<position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
|
||||
kp="40" ctrlrange="-1.57 1.57"/>
|
||||
<position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
|
||||
kp="40" ctrlrange="-1.57 1.57"/>
|
||||
<position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
|
||||
kp="30" ctrlrange="-1.57 1.57"/>
|
||||
<position name="L_ELBOW_R_pos" joint="L_ELBOW_R"
|
||||
kp="30" ctrlrange="-2.09 0.0"/>
|
||||
<position name="L_WRIST_P_pos" joint="L_WRIST_P"
|
||||
kp="20" ctrlrange="-1.57 1.57"/>
|
||||
<position name="L_WRIST_Y_pos" joint="L_WRIST_Y"
|
||||
kp="20" ctrlrange="-1.57 1.57"/>
|
||||
<position name="L_WRIST_R_pos" joint="L_WRIST_R"
|
||||
kp="20" ctrlrange="-1.57 1.57"/>
|
||||
|
||||
<!-- 右臂：位置控制 -->
|
||||
<position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
|
||||
kp="40" ctrlrange="-1.57 1.57"/>
|
||||
<position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
|
||||
kp="40" ctrlrange="-1.57 1.57"/>
|
||||
<position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
|
||||
kp="30" ctrlrange="-1.57 1.57"/>
|
||||
<position name="R_ELBOW_R_pos" joint="R_ELBOW_R"
|
||||
kp="40" ctrlrange="0 2.05"/>
|
||||
<position name="R_WRIST_P_pos" joint="R_WRIST_P"
|
||||
kp="20" ctrlrange="-1.57 1.57"/>
|
||||
<position name="R_WRIST_Y_pos" joint="R_WRIST_Y"
|
||||
kp="20" ctrlrange="-1.57 1.57"/>
|
||||
<position name="R_WRIST_R_pos" joint="R_WRIST_R"
|
||||
kp="20" ctrlrange="-1.57 1.57"/>
|
||||
</actuator>
|
||||
|
||||
|
||||
</mujoco>
|
||||
@ -1,5 +1,11 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="left_arm">
|
||||
<mujoco>
|
||||
<compiler
|
||||
meshdir="meshes"
|
||||
balanceinertia="true"
|
||||
discardvisual="false" />
|
||||
</mujoco>
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
@ -12,7 +18,7 @@
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="0.2" length="1.2"/>
|
||||
<cylinder radius="0.05" length="1.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
|
||||
63
config/robot_description/hc_description/left_arm.xml
Normal file
63
config/robot_description/hc_description/left_arm.xml
Normal file
@ -0,0 +1,63 @@
|
||||
<mujoco model="left_arm">
|
||||
<compiler angle="radian" meshdir="meshes/"/>
|
||||
|
||||
<asset>
|
||||
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
|
||||
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
|
||||
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
|
||||
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
|
||||
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
|
||||
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
|
||||
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
|
||||
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom size="0.05 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
|
||||
<geom size="0.05 0.6" type="cylinder"/>
|
||||
<geom pos="0 0 0.6" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||||
<geom pos="0 0 0.6" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
|
||||
<body name="L_SHOULDER_P_S" pos="0 0.0945 0.642">
|
||||
<inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||||
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-100 100"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
|
||||
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
|
||||
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
|
||||
<inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||||
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-100 100"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
|
||||
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
|
||||
<inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||||
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-49 49"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
|
||||
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
|
||||
<inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||||
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-49 49"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
|
||||
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
|
||||
<inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||||
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
|
||||
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
|
||||
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
|
||||
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||||
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
|
||||
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
|
||||
<inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
|
||||
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/base_link.STL
Normal file
BIN
config/robot_description/hc_description/meshes/base_link.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_index_1.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_index_1.STL
Normal file
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_index_2.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_index_2.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_ring_1.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_ring_1.STL
Normal file
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_ring_2.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_ring_2.STL
Normal file
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_thumb_1.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_thumb_1.STL
Normal file
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_thumb_2.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_thumb_2.STL
Normal file
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_thumb_3.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_thumb_3.STL
Normal file
Binary file not shown.
BIN
config/robot_description/hc_description/meshes/right_thumb_4.STL
Normal file
BIN
config/robot_description/hc_description/meshes/right_thumb_4.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,6 +1,12 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
|
||||
<robot name="right_arm">
|
||||
<mujoco>
|
||||
<compiler
|
||||
meshdir="meshes"
|
||||
balanceinertia="true"
|
||||
discardvisual="false" />
|
||||
</mujoco>
|
||||
<link name="PELVIS_S">
|
||||
<inertial>
|
||||
<origin xyz="3.78529e-05 3.81781e-07 0.0386396" rpy="0 0 0"/>
|
||||
|
||||
63
config/robot_description/hc_description/right_arm.xml
Normal file
63
config/robot_description/hc_description/right_arm.xml
Normal file
@ -0,0 +1,63 @@
|
||||
<mujoco model="right_arm">
|
||||
<compiler angle="radian" meshdir="meshes/"/>
|
||||
|
||||
<asset>
|
||||
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
|
||||
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
|
||||
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
|
||||
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
|
||||
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
|
||||
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
|
||||
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
|
||||
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
|
||||
<geom type="mesh" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
|
||||
<body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
|
||||
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
|
||||
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-1.57 1.57" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
|
||||
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
|
||||
<inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
|
||||
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-2 2" actuatorfrcrange="-120 120"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
|
||||
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
|
||||
<inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
|
||||
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="0 3.14" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
|
||||
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
|
||||
<inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
|
||||
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.18" actuatorfrcrange="-80 80"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
|
||||
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
|
||||
<inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
|
||||
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 0" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
|
||||
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
|
||||
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
|
||||
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="0 0.78" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
|
||||
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
|
||||
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
|
||||
<inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.204366" diaginertia="0.00027189 0.000186086 0.000130629"/>
|
||||
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
|
||||
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
|
||||
<geom size="0.002" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 0 1"/>
|
||||
<geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" rgba="0 1 0 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
File diff suppressed because it is too large
Load Diff
128
data/ik_psi_sweep.csv
Normal file
128
data/ik_psi_sweep.csv
Normal file
@ -0,0 +1,128 @@
|
||||
psi,q1,q2,q3,q4,q5,q6,q7
|
||||
0.000000000,0.002038980,1.340620000,0.000000000,0.522261000,0.000000000,-0.000210733,-0.094236400
|
||||
0.000000000,0.578202939,0.486316887,0.740005579,1.570796327,0.625727922,0.531578664,0.175673351
|
||||
0.000000000,0.440761487,0.565335165,0.964336466,1.570796327,0.491292823,0.442355133,0.112628007
|
||||
0.000000000,0.371238784,0.622365950,1.090264075,1.575500039,0.409159634,0.381149325,0.079289637
|
||||
0.000000000,0.330463834,0.665306397,1.171413265,1.580187790,0.353118626,0.336100996,0.059064268
|
||||
0.000000000,0.303629491,0.698061413,1.228156884,1.584859682,0.312714061,0.302027645,0.045779819
|
||||
0.000000000,0.284432039,0.723308206,1.269901344,1.589515814,0.282624592,0.275821335,0.036565409
|
||||
0.000000000,0.269759165,0.742935173,1.301736480,1.594156283,0.259722475,0.255419601,0.029896996
|
||||
0.000000000,0.257917625,0.758312994,1.326716496,1.598781185,0.241997632,0.239372235,0.024895220
|
||||
0.000000000,0.247921267,0.770456663,1.346806717,1.603390613,0.228082340,0.226624470,0.021022719
|
||||
0.000000000,0.239166095,0.780127414,1.363333864,1.607984658,0.217010934,0.216393752,0.017936958
|
||||
0.000000000,0.231267578,0.787900642,1.377223781,1.612563410,0.208084987,0.208092599,0.015412337
|
||||
0.000000000,0.223973270,0.794213370,1.389137788,1.617126957,0.200791220,0.201276831,0.013296323
|
||||
0.000000000,0.217113255,0.799398540,1.399556180,1.621675385,0.194748299,0.195609113,0.011483536
|
||||
0.000000000,0.210570785,0.803710343,1.408832069,1.626208778,0.189670728,0.190832474,0.009899847
|
||||
0.000000000,0.204264237,0.807343170,1.417227480,1.630727217,0.185343528,0.186750724,0.008492307
|
||||
0.000000000,0.198135690,0.810445914,1.424938288,1.635230785,0.181604070,0.183213775,0.007222605
|
||||
0.000000000,0.192143504,0.813132804,1.432111826,1.639719559,0.178328830,0.180106596,0.006062701
|
||||
0.000000000,0.186257388,0.815491634,1.438859529,1.644193617,0.175423635,0.177340836,0.004991872
|
||||
0.000000000,0.180455066,0.817590024,1.445266121,1.648653034,0.172816415,0.174848488,0.003994663
|
||||
0.000000000,0.174719980,0.819480182,1.451396347,1.653097884,0.170451797,0.172577060,0.003059449
|
||||
0.000000000,0.169039695,0.821202537,1.457299956,1.657528240,0.168287051,0.170485916,0.002177423
|
||||
0.000000000,0.163404770,0.822788501,1.463015392,1.661944171,0.166289047,0.168543487,0.001341855
|
||||
0.000000000,0.157807954,0.824262581,1.468572550,1.666345748,0.164431955,0.166725144,0.000547564
|
||||
0.000000000,0.152243613,0.825643979,1.473994836,1.670733038,0.162695519,0.165011583,-0.000209470
|
||||
0.000000000,0.146707313,0.826947825,1.479300719,1.675106107,0.161063745,0.163387594,-0.000932394
|
||||
0.000000000,0.141195517,0.828186109,1.484504889,1.679465019,0.159523905,0.161841116,-0.001623691
|
||||
0.000000000,0.135705364,0.829368399,1.489619141,1.683809837,0.158065790,0.160362521,-0.002285340
|
||||
0.000000000,0.130234505,0.830502387,1.494653037,1.688140624,0.156681132,0.158944065,-0.002918932
|
||||
0.000000000,0.124780985,0.831594308,1.499614410,1.692457439,0.155363175,0.157579468,-0.003525759
|
||||
0.000000000,0.119343153,0.832649258,1.504509746,1.696760341,0.154106335,0.156263589,-0.004106883
|
||||
0.000000000,0.113919592,0.833671449,1.509344478,1.701049387,0.152905953,0.154992182,-0.004663182
|
||||
0.000000000,0.108509076,0.834664389,1.514123204,1.705324634,0.151758094,0.153761706,-0.005195391
|
||||
0.000000000,0.103110526,0.835631035,1.518849866,1.709586135,0.150659401,0.152569174,-0.005704133
|
||||
0.000000000,0.097722982,0.836573903,1.523527872,1.713833945,0.149606979,0.151412044,-0.006189938
|
||||
0.000000000,0.092345584,0.837495157,1.528160207,1.718068116,0.148598304,0.150288131,-0.006653263
|
||||
0.000000000,0.086977554,0.838396676,1.532749505,1.722288697,0.147631153,0.149195535,-0.007094505
|
||||
0.000000000,0.081618181,0.839280108,1.537298113,1.726495738,0.146703554,0.148132591,-0.007514010
|
||||
0.000000000,0.076266813,0.840146913,1.541808142,1.730689288,0.145813738,0.147097827,-0.007912084
|
||||
0.000000000,0.070922846,0.840998392,1.546281501,1.734869392,0.144960109,0.146089928,-0.008288996
|
||||
0.000000000,0.065585720,0.841835716,1.550719930,1.739036098,0.144141215,0.145107715,-0.008644986
|
||||
0.000000000,0.060254914,0.842659947,1.555125023,1.743189448,0.143355729,0.144150120,-0.008980270
|
||||
0.000000000,0.054929937,0.843472051,1.559498249,1.747329486,0.142602429,0.143216170,-0.009295039
|
||||
0.000000000,0.049610331,0.844272918,1.563840968,1.751456254,0.141880187,0.142304975,-0.009589466
|
||||
0.000000000,0.044295663,0.845063363,1.568154441,1.755569792,0.141187955,0.141415716,-0.009863707
|
||||
0.000000000,0.038985523,0.845844147,1.572439847,1.759670141,0.140524756,0.140547637,-0.010117902
|
||||
0.000000000,0.033679525,0.846615971,1.576698286,1.763757337,0.139889677,0.139700037,-0.010352177
|
||||
0.000000000,0.028377300,0.847379495,1.580930790,1.767831419,0.139281864,0.138872263,-0.010566647
|
||||
0.000000000,0.023078501,0.848135333,1.585138328,1.771892423,0.138700512,0.138063706,-0.010761414
|
||||
0.000000000,0.017782795,0.848884063,1.589321815,1.775940383,0.138144864,0.137273797,-0.010936570
|
||||
0.000000000,0.012489864,0.849626229,1.593482110,1.779975333,0.137614205,0.136502000,-0.011092198
|
||||
0.000000000,0.007199406,0.850362343,1.597620030,1.783997306,0.137107858,0.135747816,-0.011228373
|
||||
0.000000000,0.001911132,0.851092890,1.601736342,1.788006333,0.136625184,0.135010771,-0.011345161
|
||||
0.000000000,-0.003375237,0.851818328,1.605831779,1.792002444,0.136165576,0.134290419,-0.011442619
|
||||
0.000000000,-0.008659966,0.852539094,1.609907031,1.795985670,0.135728457,0.133586342,-0.011520800
|
||||
0.000000000,-0.013943309,0.853255600,1.613962757,1.799956038,0.135313278,0.132898140,-0.011579750
|
||||
0.000000000,-0.019225512,0.853968239,1.617999581,1.803913576,0.134919519,0.132225440,-0.011619505
|
||||
0.000000000,-0.024506812,0.854677386,1.622018097,1.807858309,0.134546681,0.131567884,-0.011640101
|
||||
0.000000000,-0.029787434,0.855383399,1.626018873,1.811790264,0.134194290,0.130925134,-0.011641565
|
||||
0.000000000,-0.035067598,0.856086617,1.630002447,1.815709463,0.133861895,0.130296872,-0.011623919
|
||||
0.000000000,-0.040347515,0.856787368,1.633969333,1.819615930,0.133549062,0.129682792,-0.011587182
|
||||
0.000000000,-0.045627388,0.857485962,1.637920022,1.823509687,0.133255378,0.129082605,-0.011531367
|
||||
0.000000000,-0.050907414,0.858182698,1.641854983,1.827390756,0.132980448,0.128496037,-0.011456482
|
||||
0.000000000,-0.056187785,0.858877862,1.645774662,1.831259155,0.132723892,0.127922826,-0.011362533
|
||||
0.000000000,-0.061468683,0.859571728,1.649679488,1.835114905,0.132485349,0.127362723,-0.011249521
|
||||
0.000000000,-0.066750288,0.860264561,1.653569869,1.838958023,0.132264471,0.126815490,-0.011117442
|
||||
0.000000000,-0.072032772,0.860956612,1.657446195,1.842788526,0.132060923,0.126280903,-0.010966291
|
||||
0.000000000,-0.077316303,0.861648126,1.661308841,1.846606431,0.131874386,0.125758746,-0.010796058
|
||||
0.000000000,-0.082601044,0.862339336,1.665158164,1.850411752,0.131704553,0.125248813,-0.010606730
|
||||
0.000000000,-0.087887153,0.863030468,1.668994507,1.854204504,0.131551128,0.124750909,-0.010398291
|
||||
0.000000000,-0.093174784,0.863721739,1.672818198,1.857984700,0.131413827,0.124264846,-0.010170721
|
||||
0.000000000,-0.098464085,0.864413358,1.676629550,1.861752352,0.131292378,0.123790447,-0.009923998
|
||||
0.000000000,-0.103755203,0.865105528,1.680428865,1.865507472,0.131186518,0.123327542,-0.009658099
|
||||
0.000000000,-0.109048278,0.865798443,1.684216430,1.869250069,0.131095995,0.122875967,-0.009372995
|
||||
0.000000000,-0.114343449,0.866492291,1.687992522,1.872980154,0.131020565,0.122435568,-0.009068657
|
||||
0.000000000,-0.119640850,0.867187256,1.691757406,1.876697735,0.130959994,0.122006196,-0.008745051
|
||||
0.000000000,-0.124940612,0.867883512,1.695511334,1.880402820,0.130914057,0.121587711,-0.008402144
|
||||
0.000000000,-0.130242862,0.868581230,1.699254550,1.884095415,0.130882535,0.121179977,-0.008039898
|
||||
0.000000000,-0.135547726,0.869280574,1.702987286,1.887775526,0.130865219,0.120782865,-0.007658274
|
||||
0.000000000,-0.140855324,0.869981705,1.706709765,1.891443158,0.130861907,0.120396252,-0.007257229
|
||||
0.000000000,-0.146165776,0.870684776,1.710422202,1.895098315,0.130872404,0.120020021,-0.006836721
|
||||
0.000000000,-0.151479199,0.871389938,1.714124799,1.898741001,0.130896520,0.119654058,-0.006396704
|
||||
0.000000000,-0.156795705,0.872097336,1.717817754,1.902371216,0.130934076,0.119298259,-0.005937130
|
||||
0.000000000,-0.162115406,0.872807111,1.721501253,1.905988964,0.130984894,0.118952519,-0.005457950
|
||||
0.000000000,-0.167438410,0.873519398,1.725175477,1.909594244,0.131048805,0.118616741,-0.004959112
|
||||
0.000000000,-0.172764825,0.874234332,1.728840596,1.913187056,0.131125646,0.118290833,-0.004440562
|
||||
0.000000000,-0.178094754,0.874952041,1.732496776,1.916767399,0.131215259,0.117974706,-0.003902247
|
||||
0.000000000,-0.183428299,0.875672650,1.736144172,1.920335270,0.131317489,0.117668276,-0.003344109
|
||||
0.000000000,-0.188765561,0.876396281,1.739782936,1.923890667,0.131432190,0.117371461,-0.002766090
|
||||
0.000000000,-0.194106637,0.877123052,1.743413209,1.927433586,0.131559218,0.117084186,-0.002168129
|
||||
0.000000000,-0.199451624,0.877853078,1.747035130,1.930964022,0.131698435,0.116806378,-0.001550166
|
||||
0.000000000,-0.204800616,0.878586471,1.750648827,1.934481969,0.131849707,0.116537967,-0.000912137
|
||||
0.000000000,-0.210153705,0.879323339,1.754254425,1.937987422,0.132012905,0.116278888,-0.000253978
|
||||
0.000000000,-0.215510983,0.880063789,1.757852042,1.941480372,0.132187903,0.116029078,0.000424378
|
||||
0.000000000,-0.220872538,0.880807923,1.761441791,1.944960813,0.132374581,0.115788479,0.001122999
|
||||
0.000000000,-0.226238458,0.881555841,1.765023777,1.948428734,0.132572820,0.115557034,0.001841952
|
||||
0.000000000,-0.231608829,0.882307642,1.768598103,1.951884127,0.132782508,0.115334690,0.002581309
|
||||
0.000000000,-0.236983736,0.883063421,1.772164864,1.955326981,0.133003534,0.115121397,0.003341141
|
||||
0.000000000,-0.242363261,0.883823269,1.775724150,1.958757284,0.133235792,0.114917109,0.004121521
|
||||
0.000000000,-0.247747485,0.884587277,1.779276047,1.962175024,0.133479179,0.114721780,0.004922524
|
||||
0.000000000,-0.253136490,0.885355532,1.782820636,1.965580188,0.133733595,0.114535369,0.005744225
|
||||
0.000000000,-0.258530353,0.886128121,1.786357992,1.968972763,0.133998943,0.114357837,0.006586701
|
||||
0.000000000,-0.263929153,0.886905126,1.789888186,1.972352734,0.134275129,0.114189147,0.007450029
|
||||
0.000000000,-0.269332966,0.887686629,1.793411286,1.975720085,0.134562062,0.114029264,0.008334290
|
||||
0.000000000,-0.274741865,0.888472709,1.796927352,1.979074800,0.134859656,0.113878158,0.009239563
|
||||
0.000000000,-0.280155926,0.889263443,1.800436443,1.982416862,0.135167823,0.113735798,0.010165930
|
||||
0.000000000,-0.285575221,0.890058905,1.803938611,1.985746253,0.135486482,0.113602158,0.011113473
|
||||
0.000000000,-0.290999820,0.890859169,1.807433906,1.989062955,0.135815553,0.113477212,0.012082277
|
||||
0.000000000,-0.296429795,0.891664305,1.810922373,1.992366949,0.136154957,0.113360936,0.013072425
|
||||
0.000000000,-0.301865215,0.892474384,1.814404052,1.995658213,0.136504620,0.113253312,0.014084004
|
||||
0.000000000,-0.307306147,0.893289473,1.817878980,1.998936727,0.136864469,0.113154318,0.015117101
|
||||
0.000000000,-0.312752658,0.894109637,1.821347191,2.002202469,0.137234433,0.113063939,0.016171803
|
||||
0.000000000,-0.318204815,0.894934939,1.824808713,2.005455417,0.137614443,0.112982160,0.017248199
|
||||
0.000000000,-0.323662683,0.895765444,1.828263571,2.008695547,0.138004434,0.112908967,0.018346379
|
||||
0.000000000,-0.329126324,0.896601210,1.831711788,2.011922835,0.138404340,0.112844349,0.019466434
|
||||
0.000000000,-0.334595804,0.897442297,1.835153381,2.015137257,0.138814099,0.112788297,0.020608455
|
||||
0.000000000,-0.340071182,0.898288761,1.838588365,2.018338785,0.139233651,0.112740803,0.021772535
|
||||
0.000000000,-0.345552521,0.899140659,1.842016750,2.021527395,0.139662937,0.112701861,0.022958766
|
||||
0.000000000,-0.351039881,0.899998044,1.845438544,2.024703059,0.140101899,0.112671467,0.024167244
|
||||
0.000000000,-0.356533320,0.900860968,1.848853751,2.027865749,0.140550484,0.112649617,0.025398064
|
||||
0.000000000,-0.362032897,0.901729483,1.852262372,2.031015436,0.141008637,0.112636311,0.026651320
|
||||
0.000000000,-0.367538669,0.902603637,1.855664402,2.034152091,0.141476307,0.112631550,0.027927111
|
||||
0.000000000,-0.373050693,0.903483479,1.859059837,2.037275683,0.141953442,0.112635335,0.029225533
|
||||
0.000000000,-0.378569025,0.904369054,1.862448667,2.040386181,0.142439995,0.112647669,0.030546685
|
||||
0.000000000,-0.384093719,0.905260407,1.865830879,2.043483554,0.142935918,0.112668558,0.031890665
|
||||
0.000000000,-0.389624828,0.906157581,1.869206457,2.046567770,0.143441165,0.112698008,0.033257574
|
||||
0.000000000,-0.395162407,0.907060618,1.872575383,2.049638794,0.143955692,0.112736027,0.034647511
|
||||
|
0
protos/cmvr/config/opt_psi_limit_bias_config.proto
Normal file
0
protos/cmvr/config/opt_psi_limit_bias_config.proto
Normal file
0
protos/cmvr/config/pinocchio_dls_ik_config.proto
Normal file
0
protos/cmvr/config/pinocchio_dls_ik_config.proto
Normal file
17
protos/cmvr/config/pinocchio_qp_ik_config.proto
Normal file
17
protos/cmvr/config/pinocchio_qp_ik_config.proto
Normal file
@ -0,0 +1,17 @@
|
||||
syntax = "proto3";
|
||||
|
||||
package cmvr.config;
|
||||
|
||||
// 只管 QP 这个 IK 的参数
|
||||
message PinocchioQpIKConfig {
|
||||
string urdf_path = 1;
|
||||
string base_frame_name = 2;
|
||||
string flange_frame_name = 3;
|
||||
string tcp_frame_name = 4;
|
||||
|
||||
double lambda = 5;
|
||||
double w_posrot = 6;
|
||||
int32 max_iters = 7;
|
||||
double tol = 8;
|
||||
double qp_time_limit = 9;
|
||||
}
|
||||
BIN
script/tool/L_WRIST_R_S.STL
Normal file
BIN
script/tool/L_WRIST_R_S.STL
Normal file
Binary file not shown.
BIN
script/tool/R_WRIST_R_S.STL
Normal file
BIN
script/tool/R_WRIST_R_S.STL
Normal file
Binary file not shown.
BIN
script/tool/R_WRIST_R_S_low.STL
Normal file
BIN
script/tool/R_WRIST_R_S_low.STL
Normal file
Binary file not shown.
22
script/tool/de_stl_faces.py
Normal file
22
script/tool/de_stl_faces.py
Normal file
@ -0,0 +1,22 @@
|
||||
import trimesh
|
||||
|
||||
path = "R_WRIST_R_S.STL"
|
||||
|
||||
obj = trimesh.load(path)
|
||||
|
||||
if isinstance(obj, trimesh.Scene):
|
||||
mesh = trimesh.util.concatenate(list(obj.geometry.values()))
|
||||
else:
|
||||
mesh = obj
|
||||
|
||||
print("原始面数:", len(mesh.faces))
|
||||
|
||||
# 想删掉 80% 的面（保留 20%）
|
||||
target_reduction = 0.8 # 0~1 之间
|
||||
|
||||
mesh_simplified = mesh.simplify_quadric_decimation(target_reduction)
|
||||
print("简化后面数:", len(mesh_simplified.faces))
|
||||
|
||||
out_path = "R_WRIST_R_S_low.STL"
|
||||
mesh_simplified.export(out_path)
|
||||
print("已导出:", out_path)
|
||||
@ -10,3 +10,4 @@ add_subdirectory(planner)
|
||||
|
||||
add_subdirectory(ik_solver)
|
||||
add_subdirectory(data_center)
|
||||
add_subdirectory(simulate)
|
||||
@ -5,10 +5,11 @@
|
||||
#define M2CM 100
|
||||
#define CM2M 0.01
|
||||
|
||||
#define PI 3.141592653589793
|
||||
#define HALF_PI 1.570796326794897
|
||||
#define TWO_PI 6.283185307179586
|
||||
|
||||
static constexpr double EPS = 1e-9;
|
||||
|
||||
|
||||
// 计数：最多 10 个
|
||||
#define _ArgCount(_0,_1,_2,_3,_4,_5,_6,_7,_8,_9, COUNT, ...) COUNT
|
||||
|
||||
424
src/common/utils/math/qp_solver.h
Normal file
424
src/common/utils/math/qp_solver.h
Normal file
@ -0,0 +1,424 @@
|
||||
//
|
||||
// Created by lgv on 12/2/25.
|
||||
//
|
||||
|
||||
#ifndef CMVR_MATH_QP_SOLVER_H
|
||||
#define CMVR_MATH_QP_SOLVER_H
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <exception>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <unordered_map>
|
||||
#include <iostream>
|
||||
|
||||
#include <eigen3/Eigen/Core>
|
||||
#include <OsqpEigen/OsqpEigen.h>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
class QPSolverException : public std::exception {
|
||||
public:
|
||||
static constexpr unsigned int kStatusOffset = 100;
|
||||
|
||||
explicit QPSolverException(int error_code);
|
||||
|
||||
const char* what() const noexcept override;
|
||||
|
||||
int code() const noexcept;
|
||||
|
||||
static std::string GenerateMessage(int code);
|
||||
|
||||
private:
|
||||
int error_code_;
|
||||
std::string message_;
|
||||
};
|
||||
|
||||
inline QPSolverException::QPSolverException(int error_code)
|
||||
: error_code_(error_code)
|
||||
, message_(GenerateMessage(error_code))
|
||||
{
|
||||
}
|
||||
|
||||
inline const char* QPSolverException::what() const noexcept {
|
||||
return message_.c_str();
|
||||
}
|
||||
|
||||
inline int QPSolverException::code() const noexcept {
|
||||
return error_code_;
|
||||
}
|
||||
|
||||
inline std::string QPSolverException::GenerateMessage(int code) {
|
||||
static const std::unordered_map<int, std::string> error_messages = {
|
||||
{OSQP_DATA_VALIDATION_ERROR, "Data validation error"},
|
||||
{OSQP_SETTINGS_VALIDATION_ERROR, "Settings validation error"},
|
||||
#ifdef OSQP_EIGEN_OSQP_IS_V1
|
||||
{OSQP_ALGEBRA_LOAD_ERROR, "Linear system solver load error"},
|
||||
#else
|
||||
{OSQP_LINSYS_SOLVER_LOAD_ERROR, "Linear system solver load error"},
|
||||
#endif
|
||||
{OSQP_LINSYS_SOLVER_INIT_ERROR, "Linear system solver initialization error"},
|
||||
{OSQP_NONCVX_ERROR, "Non-convex error"},
|
||||
{OSQP_MEM_ALLOC_ERROR, "Memory allocation error"},
|
||||
{OSQP_WORKSPACE_NOT_INIT_ERROR, "Workspace not initialized"},
|
||||
|
||||
{QPSolverException::kStatusOffset + OSQP_SOLVED, "Solved"},
|
||||
{QPSolverException::kStatusOffset + OSQP_SOLVED_INACCURATE, "Solved inaccurate"},
|
||||
{QPSolverException::kStatusOffset + OSQP_PRIMAL_INFEASIBLE, "Primal infeasible"},
|
||||
{QPSolverException::kStatusOffset + OSQP_PRIMAL_INFEASIBLE_INACCURATE, "Primal infeasible inaccurate"},
|
||||
{QPSolverException::kStatusOffset + OSQP_DUAL_INFEASIBLE, "Dual infeasible"},
|
||||
{QPSolverException::kStatusOffset + OSQP_DUAL_INFEASIBLE_INACCURATE, "Dual infeasible inaccurate"},
|
||||
{QPSolverException::kStatusOffset + OSQP_MAX_ITER_REACHED, "Maximum iterations reached"},
|
||||
{QPSolverException::kStatusOffset + OSQP_TIME_LIMIT_REACHED, "Run time limit reached"},
|
||||
{QPSolverException::kStatusOffset + OSQP_NON_CVX, "Problem non convex"},
|
||||
{QPSolverException::kStatusOffset + OSQP_SIGINT, "Interrupted"},
|
||||
{QPSolverException::kStatusOffset + OSQP_UNSOLVED, "Unsolved"},
|
||||
|
||||
{-1, "General solver error or matrix update failure"},
|
||||
};
|
||||
|
||||
auto it = error_messages.find(code);
|
||||
if (it != error_messages.end()) {
|
||||
return it->second;
|
||||
}
|
||||
return "Unknown solver error (code: " + std::to_string(code) + ")";
|
||||
}
|
||||
|
||||
|
||||
|
||||
class QPSolverImpl {
|
||||
public:
|
||||
QPSolverImpl() = default;
|
||||
~QPSolverImpl() = default;
|
||||
|
||||
void SetupImpl(int n_var, int n_const, double time_limit = 2e-3);
|
||||
void InitFunctionImpl();
|
||||
void AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b);
|
||||
void SetCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b);
|
||||
void SetConstraintsFunctionImpl(const Eigen::MatrixXd &A,
|
||||
const Eigen::VectorXd &lb,
|
||||
const Eigen::VectorXd &ub);
|
||||
void SetPrimalVariableImpl(const Eigen::VectorXd &pv);
|
||||
void ResetIsFirstImpl();
|
||||
Eigen::VectorXd SolveImpl();
|
||||
Eigen::MatrixXd GetACostImpl() const;
|
||||
Eigen::VectorXd GetBCostImpl() const;
|
||||
Eigen::MatrixXd GetAConstImpl() const;
|
||||
Eigen::VectorXd GetLowerBoundImpl() const;
|
||||
Eigen::VectorXd GetUpperBoundImpl() const;
|
||||
|
||||
private:
|
||||
OsqpEigen::Solver solver_;
|
||||
int n_var_{0};
|
||||
int n_const_{0};
|
||||
int err_code_{0};
|
||||
|
||||
int is_first_{1};
|
||||
|
||||
int n_hessian_element_{0};
|
||||
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_cost_;
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> b_cost_;
|
||||
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_const_;
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> lb_;
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> ub_;
|
||||
|
||||
Eigen::SparseMatrix<double> hessian_;
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> gradient_;
|
||||
|
||||
Eigen::SparseMatrix<double> linearMatrix_;
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> lowerBound_;
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> upperBound_;
|
||||
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> primal_variable_for_warmstart_;
|
||||
};
|
||||
|
||||
inline void QPSolverImpl::SetupImpl(int n_var, int n_const, double time_limit) {
|
||||
n_var_ = n_var;
|
||||
n_const_ = n_const;
|
||||
|
||||
solver_.settings()->setWarmStart(true);
|
||||
solver_.settings()->setVerbosity(false);
|
||||
solver_.settings()->setTimeLimit(time_limit);
|
||||
|
||||
solver_.data()->setNumberOfVariables(n_var_);
|
||||
solver_.data()->setNumberOfConstraints(n_const_);
|
||||
|
||||
primal_variable_for_warmstart_.resize(n_var_, 1);
|
||||
primal_variable_for_warmstart_.setZero();
|
||||
|
||||
is_first_ = true;
|
||||
n_hessian_element_ = 0;
|
||||
}
|
||||
|
||||
inline void QPSolverImpl::InitFunctionImpl() {
|
||||
A_cost_.resize(0, n_var_);
|
||||
b_cost_.resize(0, 1);
|
||||
|
||||
A_const_.resize(0, n_var_);
|
||||
lb_.resize(0, 1);
|
||||
ub_.resize(0, 1);
|
||||
|
||||
A_cost_.setZero();
|
||||
b_cost_.setZero();
|
||||
|
||||
A_const_.setZero();
|
||||
lb_.setZero();
|
||||
ub_.setZero();
|
||||
}
|
||||
|
||||
inline void QPSolverImpl::AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) {
|
||||
if (A.rows() != b.rows()) {
|
||||
std::cerr << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): "
|
||||
<< A.rows() << ", b.rows(): " << b.rows() << ")" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (b.cols() != 1) {
|
||||
std::cerr << "OSQP Solver Add cost function failed: Size issue: b (Nx1)" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
int n_rows_old = static_cast<int>(A_cost_.rows());
|
||||
int n_rows_add = static_cast<int>(A.rows());
|
||||
|
||||
if (n_rows_old == 0) {
|
||||
A_cost_.resize(n_rows_add, A.cols());
|
||||
b_cost_.resize(n_rows_add, 1);
|
||||
A_cost_.bottomRows(n_rows_add) = A;
|
||||
b_cost_.bottomRows(n_rows_add) = b;
|
||||
} else {
|
||||
A_cost_.conservativeResize(n_rows_old + n_rows_add, Eigen::NoChange);
|
||||
b_cost_.conservativeResize(n_rows_old + n_rows_add, Eigen::NoChange);
|
||||
A_cost_.bottomRows(n_rows_add) = A;
|
||||
b_cost_.bottomRows(n_rows_add) = b;
|
||||
}
|
||||
}
|
||||
|
||||
inline void QPSolverImpl::SetCostFunctionImpl(const Eigen::MatrixXd &A,
|
||||
const Eigen::VectorXd &b) {
|
||||
// 代价：min ||A x - b||^2
|
||||
// => H = A^T A, g = -A^T b
|
||||
hessian_ = (A.transpose() * A).sparseView();
|
||||
gradient_ = -A.transpose() * b;
|
||||
}
|
||||
|
||||
inline void QPSolverImpl::SetConstraintsFunctionImpl(const Eigen::MatrixXd &A,
|
||||
const Eigen::VectorXd &lb,
|
||||
const Eigen::VectorXd &ub) {
|
||||
// 约束：lb <= A x <= ub
|
||||
A_const_ = A;
|
||||
lb_ = lb;
|
||||
ub_ = ub;
|
||||
|
||||
linearMatrix_ = A.sparseView();
|
||||
lowerBound_ = lb;
|
||||
upperBound_ = ub;
|
||||
}
|
||||
|
||||
inline void QPSolverImpl::SetPrimalVariableImpl(const Eigen::VectorXd &pv) {
|
||||
primal_variable_for_warmstart_ = pv;
|
||||
}
|
||||
|
||||
inline void QPSolverImpl::ResetIsFirstImpl() {
|
||||
is_first_ = true;
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolverImpl::SolveImpl() {
|
||||
bool need_init = false;
|
||||
|
||||
if (!is_first_) {
|
||||
if (n_hessian_element_ == hessian_.nonZeros()) {
|
||||
if (!solver_.updateHessianMatrix(hessian_)) {
|
||||
need_init = true;
|
||||
}
|
||||
if (!solver_.updateGradient(gradient_)) {
|
||||
need_init = true;
|
||||
}
|
||||
if (!solver_.updateLinearConstraintsMatrix(linearMatrix_)) {
|
||||
need_init = true;
|
||||
}
|
||||
if (!solver_.updateBounds(lowerBound_, upperBound_)) {
|
||||
need_init = true;
|
||||
}
|
||||
} else {
|
||||
need_init = true;
|
||||
}
|
||||
} else {
|
||||
need_init = true;
|
||||
}
|
||||
|
||||
if (need_init) {
|
||||
solver_.data()->clearHessianMatrix();
|
||||
solver_.data()->clearLinearConstraintsMatrix();
|
||||
solver_.clearSolver();
|
||||
|
||||
n_hessian_element_ = static_cast<int>(hessian_.nonZeros());
|
||||
|
||||
if (!solver_.data()->setHessianMatrix(hessian_)) {
|
||||
throw QPSolverException(-1);
|
||||
}
|
||||
if (!solver_.data()->setGradient(gradient_)) {
|
||||
throw QPSolverException(-1);
|
||||
}
|
||||
if (!solver_.data()->setLinearConstraintsMatrix(linearMatrix_)) {
|
||||
throw QPSolverException(-1);
|
||||
}
|
||||
if (!solver_.data()->setLowerBound(lowerBound_)) {
|
||||
throw QPSolverException(-1);
|
||||
}
|
||||
if (!solver_.data()->setUpperBound(upperBound_)) {
|
||||
throw QPSolverException(-1);
|
||||
}
|
||||
|
||||
if (!solver_.initSolver()) {
|
||||
throw QPSolverException(-1);
|
||||
}
|
||||
|
||||
is_first_ = false;
|
||||
}
|
||||
|
||||
solver_.setPrimalVariable(primal_variable_for_warmstart_);
|
||||
|
||||
const auto result = solver_.solveProblem();
|
||||
if (result != OsqpEigen::ErrorExitFlag::NoError) {
|
||||
throw QPSolverException(static_cast<int>(result));
|
||||
}
|
||||
if (solver_.getStatus() != OsqpEigen::Status::Solved) {
|
||||
throw QPSolverException(
|
||||
static_cast<int>(QPSolverException::kStatusOffset) +
|
||||
static_cast<int>(solver_.getStatus()));
|
||||
}
|
||||
|
||||
return solver_.getSolution();
|
||||
}
|
||||
|
||||
inline Eigen::MatrixXd QPSolverImpl::GetACostImpl() const {
|
||||
return A_cost_;
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolverImpl::GetBCostImpl() const {
|
||||
return b_cost_;
|
||||
}
|
||||
|
||||
inline Eigen::MatrixXd QPSolverImpl::GetAConstImpl() const {
|
||||
return A_const_;
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolverImpl::GetLowerBoundImpl() const {
|
||||
return lb_;
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolverImpl::GetUpperBoundImpl() const {
|
||||
return ub_;
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------
|
||||
// 对外的 QPSolver 封装
|
||||
// -----------------------------------------------------------
|
||||
|
||||
class QPSolver {
|
||||
public:
|
||||
QPSolver();
|
||||
~QPSolver();
|
||||
|
||||
void Setup(int n_var, int n_const, double time_limit = 2e-3);
|
||||
|
||||
void InitFunction();
|
||||
|
||||
void AddCostFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& b);
|
||||
|
||||
void SetCostFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& b);
|
||||
|
||||
void SetConstraintsFunction(const Eigen::MatrixXd& A,
|
||||
const Eigen::VectorXd& lb,
|
||||
const Eigen::VectorXd& ub);
|
||||
|
||||
void SetPrimalVariable(const Eigen::VectorXd& pv);
|
||||
|
||||
void ResetIsFirst();
|
||||
|
||||
/**
|
||||
* Solve the QP problem
|
||||
* @return Solution
|
||||
* @throw QPSolverException
|
||||
*/
|
||||
Eigen::VectorXd Solve();
|
||||
|
||||
Eigen::MatrixXd GetACost() const; // NOLINT
|
||||
Eigen::VectorXd GetBCost() const; // NOLINT
|
||||
Eigen::MatrixXd GetAConst() const; // NOLINT
|
||||
Eigen::VectorXd GetLowerBound() const; // NOLINT
|
||||
Eigen::VectorXd GetUpperBound() const; // NOLINT
|
||||
|
||||
private:
|
||||
std::unique_ptr<QPSolverImpl> impl_;
|
||||
};
|
||||
|
||||
inline QPSolver::QPSolver() {
|
||||
impl_ = std::make_unique<QPSolverImpl>();
|
||||
}
|
||||
|
||||
inline QPSolver::~QPSolver() = default;
|
||||
|
||||
inline void QPSolver::Setup(int n_var, int n_const, double time_limit) {
|
||||
impl_->SetupImpl(n_var, n_const, time_limit);
|
||||
}
|
||||
|
||||
inline void QPSolver::InitFunction() {
|
||||
impl_->InitFunctionImpl();
|
||||
}
|
||||
|
||||
inline void QPSolver::AddCostFunction(const Eigen::MatrixXd &A,
|
||||
const Eigen::VectorXd &b) {
|
||||
impl_->AddCostFunctionImpl(A, b);
|
||||
}
|
||||
|
||||
inline void QPSolver::SetCostFunction(const Eigen::MatrixXd &A,
|
||||
const Eigen::VectorXd &b) {
|
||||
impl_->SetCostFunctionImpl(A, b);
|
||||
}
|
||||
|
||||
inline void QPSolver::SetConstraintsFunction(const Eigen::MatrixXd &A,
|
||||
const Eigen::VectorXd &lb,
|
||||
const Eigen::VectorXd &ub) {
|
||||
impl_->SetConstraintsFunctionImpl(A, lb, ub);
|
||||
}
|
||||
|
||||
inline void QPSolver::SetPrimalVariable(const Eigen::VectorXd &pv) {
|
||||
impl_->SetPrimalVariableImpl(pv);
|
||||
}
|
||||
|
||||
inline void QPSolver::ResetIsFirst() {
|
||||
impl_->ResetIsFirstImpl();
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolver::Solve() {
|
||||
return impl_->SolveImpl();
|
||||
}
|
||||
|
||||
inline Eigen::MatrixXd QPSolver::GetACost() const {
|
||||
return impl_->GetACostImpl();
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolver::GetBCost() const {
|
||||
return impl_->GetBCostImpl();
|
||||
}
|
||||
|
||||
inline Eigen::MatrixXd QPSolver::GetAConst() const {
|
||||
return impl_->GetAConstImpl();
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolver::GetLowerBound() const {
|
||||
return impl_->GetLowerBoundImpl();
|
||||
}
|
||||
|
||||
inline Eigen::VectorXd QPSolver::GetUpperBound() const {
|
||||
return impl_->GetUpperBoundImpl();
|
||||
}
|
||||
|
||||
} // namespace cmvr::math
|
||||
|
||||
#endif // CMVR_MATH_QP_SOLVER_H
|
||||
2988
src/common/utils/visualization/matplotlibcpp.h
Normal file
2988
src/common/utils/visualization/matplotlibcpp.h
Normal file
File diff suppressed because it is too large
Load Diff
@ -1,51 +1,20 @@
|
||||
find_package(glog REQUIRED)
|
||||
find_package(protobuf REQUIRED)
|
||||
|
||||
add_library(controller SHARED
|
||||
src/touch_controller.cpp
|
||||
|
||||
file(GLOB SRC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
|
||||
)
|
||||
|
||||
add_library(controller SHARED ${SRC})
|
||||
|
||||
|
||||
target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(controller PRIVATE
|
||||
target_link_libraries(controller PUBLIC
|
||||
protobuf::libprotobuf
|
||||
glog::glog
|
||||
cmvr_es::device::humanoid_robot
|
||||
|
||||
)
|
||||
|
||||
add_library(cmvr_es::ctrl::controller ALIAS controller)
|
||||
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Unit test
|
||||
# --------------------------------------------------------
|
||||
find_package(glog REQUIRED)
|
||||
find_package(protobuf REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(fcl REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
||||
)
|
||||
|
||||
link_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
||||
)
|
||||
|
||||
|
||||
add_executable(touch_controller_test
|
||||
src/touch_controller_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(touch_controller_test
|
||||
PRIVATE
|
||||
protobuf::libprotobuf
|
||||
glog::glog
|
||||
cmvr_es::device::humanoid_robot
|
||||
cmvr_es::ctrl::controller
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
@ -56,3 +25,5 @@ target_link_libraries(touch_controller_test
|
||||
cmvr_es::device_manager
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
add_library(cmvr_es::controller ALIAS controller)
|
||||
|
||||
81
src/controller/include/controller.h
Normal file
81
src/controller/include/controller.h
Normal file
@ -0,0 +1,81 @@
|
||||
//
|
||||
// Created by lgv on 11/27/25.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include "common/consts/constant.h"
|
||||
namespace cmvr {
|
||||
|
||||
/// 当前机器人状态（输入）
|
||||
struct ControlInput {
|
||||
// 关节空间
|
||||
Eigen::VectorXd q; ///< 当前关节角
|
||||
Eigen::VectorXd dq; ///< 当前关节角速度
|
||||
|
||||
// 末端任务空间（阻抗控制用，可选）
|
||||
Eigen::VectorXd x; ///< 当前末端位姿（3或6维等）
|
||||
Eigen::VectorXd dx; ///< 当前末端速度
|
||||
Eigen::MatrixXd Jx; ///< 末端雅可比
|
||||
|
||||
// 视觉空间（视觉伺服用，可选）
|
||||
Eigen::VectorXd s; ///< 当前视觉特征
|
||||
Eigen::MatrixXd Js; ///< 视觉雅可比 ∂s/∂q
|
||||
};
|
||||
|
||||
/// 控制目标（参考）
|
||||
struct ControlReference {
|
||||
// 关节空间参考（PID 等）
|
||||
Eigen::VectorXd q_d;
|
||||
Eigen::VectorXd dq_d;
|
||||
Eigen::VectorXd ddq_d;
|
||||
|
||||
// 末端任务空间参考（阻抗）
|
||||
Eigen::VectorXd x_d;
|
||||
Eigen::VectorXd dx_d;
|
||||
|
||||
// 视觉空间参考（视觉伺服）
|
||||
Eigen::VectorXd s_d;
|
||||
};
|
||||
|
||||
/// @brief 抽象控制器基类：
|
||||
class Controller {
|
||||
public:
|
||||
virtual ~Controller() = default;
|
||||
|
||||
/// 设置自由度数量
|
||||
virtual void init(int dof) = 0;
|
||||
|
||||
/// 设置控制目标（关节 / 任务空间 / 视觉），
|
||||
/// 各派生类只用自己关心的字段
|
||||
virtual void setReference(const ControlReference& ref) = 0;
|
||||
|
||||
/// 核心：根据当前状态算控制量
|
||||
virtual Eigen::VectorXd compute(const ControlInput& input, double dt) = 0;
|
||||
|
||||
// ========= 通用增益接口（可选，默认空实现） =========
|
||||
|
||||
/// 标量增益，例如视觉伺服里的 λ
|
||||
virtual void setGain(double gain) {
|
||||
UNUSED_VARIABLE(gain);
|
||||
}
|
||||
|
||||
/// PID 三个向量增益
|
||||
virtual void setGains(const Eigen::VectorXd& kp,
|
||||
const Eigen::VectorXd& ki,
|
||||
const Eigen::VectorXd& kd) {
|
||||
UNUSED_VARIABLE(kp,ki,kd);
|
||||
}
|
||||
|
||||
/// 阻抗控制用的 K, D 矩阵
|
||||
virtual void setGains(const Eigen::MatrixXd& k,
|
||||
const Eigen::MatrixXd& d) {
|
||||
UNUSED_VARIABLE(k,d);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
|
||||
|
||||
13
src/controller/include/controller_creator.h
Normal file
13
src/controller/include/controller_creator.h
Normal file
@ -0,0 +1,13 @@
|
||||
//
|
||||
// Created by lgv on 11/27/25.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_CONTROLLER_CREATOR_H
|
||||
#define CMVR_ES_CONTROLLER_CREATOR_H
|
||||
|
||||
|
||||
class controller_creator {
|
||||
};
|
||||
|
||||
|
||||
#endif //CMVR_ES_CONTROLLER_CREATOR_H
|
||||
71
src/controller/include/pid_controller.h
Normal file
71
src/controller/include/pid_controller.h
Normal file
@ -0,0 +1,71 @@
|
||||
//
|
||||
// Created by lgv on 11/27/25.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "controller/include/controller.h"
|
||||
#include <limits>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
class PidController : public Controller {
|
||||
public:
|
||||
PidController();
|
||||
|
||||
// 设置自由度+ 初始化
|
||||
void init(int dof) override;
|
||||
|
||||
// 设置目标（只用到 q_d, dq_d）
|
||||
void setReference(const ControlReference& ref) override;
|
||||
|
||||
// 核心：根据当前状态计算控制量（约定为关节力矩）
|
||||
Eigen::VectorXd compute(const ControlInput& input, double dt) override;
|
||||
|
||||
// PID 三向量增益
|
||||
void setGains(const Eigen::VectorXd& kp,
|
||||
const Eigen::VectorXd& ki,
|
||||
const Eigen::VectorXd& kd) override;
|
||||
|
||||
/// 设置积分上下限： lower[i] <= I[i] <= upper[i]
|
||||
/// upper[i] <= lower[i] 表示该关节不启用积分限幅
|
||||
void setIntegralLimits(const Eigen::VectorXd& lower,
|
||||
const Eigen::VectorXd& upper);
|
||||
|
||||
/// 设置误差死区： |error[i]| < deadzone[i] 时视为 0
|
||||
void setDeadzone(const Eigen::VectorXd& deadzone);
|
||||
|
||||
/// 设置 D 项滤波系数（简单一阶滤波）
|
||||
/// coeff <= 0 表示不启用滤波
|
||||
void setDerivativeFilterCoeff(double coeff);
|
||||
|
||||
private:
|
||||
int dof_ = 0;
|
||||
|
||||
// PID 增益
|
||||
Eigen::VectorXd kp_;
|
||||
Eigen::VectorXd ki_;
|
||||
Eigen::VectorXd kd_;
|
||||
|
||||
// 期望状态
|
||||
Eigen::VectorXd q_d_;
|
||||
Eigen::VectorXd dq_d_;
|
||||
|
||||
// 积分项、上一拍误差
|
||||
Eigen::VectorXd integralError_;
|
||||
Eigen::VectorXd prevError_;
|
||||
|
||||
// 积分上下限
|
||||
Eigen::VectorXd integralLowerLimit_;
|
||||
Eigen::VectorXd integralUpperLimit_;
|
||||
|
||||
// 死区
|
||||
Eigen::VectorXd deadzone_;
|
||||
|
||||
// D 项滤波
|
||||
double dFilterCoeff_ = 0.0; ///< 滤波系数，0 表示不开滤波
|
||||
Eigen::VectorXd dErrorFiltered_; ///< 滤波后的 dError
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
5
src/controller/src/controller_creator.cpp
Normal file
5
src/controller/src/controller_creator.cpp
Normal file
@ -0,0 +1,5 @@
|
||||
//
|
||||
// Created by lgv on 11/27/25.
|
||||
//
|
||||
|
||||
#include "../include/controller_creator.h"
|
||||
138
src/controller/src/pid_controller.cpp
Normal file
138
src/controller/src/pid_controller.cpp
Normal file
@ -0,0 +1,138 @@
|
||||
//
|
||||
// Created by lgv on 11/27/25.
|
||||
//
|
||||
|
||||
#include "controller/include/pid_controller.h"
|
||||
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
|
||||
PidController::PidController() = default;
|
||||
|
||||
void PidController::init(int dof)
|
||||
{
|
||||
dof_ = dof;
|
||||
|
||||
kp_.setZero(dof_);
|
||||
ki_.setZero(dof_);
|
||||
kd_.setZero(dof_);
|
||||
|
||||
q_d_.setZero(dof_);
|
||||
dq_d_.setZero(dof_);
|
||||
|
||||
integralError_.setZero(dof_);
|
||||
prevError_.setZero(dof_);
|
||||
|
||||
// 默认：不启用积分限幅（upper <= lower 视为禁用）
|
||||
integralLowerLimit_.setConstant(dof_, 1.0);
|
||||
integralUpperLimit_.setConstant(dof_, -1.0);
|
||||
|
||||
// 默认：无死区
|
||||
deadzone_.setZero(dof_);
|
||||
|
||||
// 默认：不启用 D 滤波
|
||||
dFilterCoeff_ = 0.0;
|
||||
dErrorFiltered_.setZero(dof_);
|
||||
}
|
||||
|
||||
void PidController::setGains(const Eigen::VectorXd& kp,
|
||||
const Eigen::VectorXd& ki,
|
||||
const Eigen::VectorXd& kd)
|
||||
{
|
||||
kp_ = kp;
|
||||
ki_ = ki;
|
||||
kd_ = kd;
|
||||
}
|
||||
|
||||
void PidController::setIntegralLimits(const Eigen::VectorXd& lower,
|
||||
const Eigen::VectorXd& upper)
|
||||
{
|
||||
integralLowerLimit_ = lower;
|
||||
integralUpperLimit_ = upper;
|
||||
}
|
||||
|
||||
void PidController::setDeadzone(const Eigen::VectorXd& deadzone)
|
||||
{
|
||||
deadzone_ = deadzone;
|
||||
}
|
||||
|
||||
void PidController::setDerivativeFilterCoeff(double coeff)
|
||||
{
|
||||
dFilterCoeff_ = coeff; // <= EPS 时在 compute 里视为不开滤波
|
||||
}
|
||||
|
||||
void PidController::setReference(const ControlReference& ref)
|
||||
{
|
||||
q_d_ = ref.q_d;
|
||||
|
||||
if (ref.dq_d.size() == q_d_.size())
|
||||
dq_d_ = ref.dq_d;
|
||||
else
|
||||
dq_d_.setZero(dof_);
|
||||
}
|
||||
|
||||
Eigen::VectorXd PidController::compute(const ControlInput& input, double dt)
|
||||
{
|
||||
const Eigen::VectorXd& q = input.q;
|
||||
const Eigen::VectorXd& dq = input.dq;
|
||||
|
||||
Eigen::VectorXd error = q_d_ - q;
|
||||
|
||||
// ===== 1. 误差死区（|e| < deadzone => e = 0） =====
|
||||
for (int i = 0; i < dof_; ++i) {
|
||||
double dz = (i < deadzone_.size()) ? deadzone_[i] : 0.0;
|
||||
if (dz > EPS && std::abs(error[i]) < dz) {
|
||||
error[i] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 2. 微分项 + 一阶滤波 =====
|
||||
Eigen::VectorXd dErrorRaw(dof_);
|
||||
Eigen::VectorXd dError(dof_);
|
||||
|
||||
if (dt > EPS) {
|
||||
dErrorRaw = (error - prevError_) / dt;
|
||||
} else {
|
||||
dErrorRaw.setZero();
|
||||
}
|
||||
|
||||
if (dFilterCoeff_ > EPS && dt > EPS) {
|
||||
// 一阶离散滤波：alpha 越接近 1，越接近原始微分
|
||||
double alpha = dFilterCoeff_ / (dFilterCoeff_ + dt);
|
||||
dErrorFiltered_ = alpha * dErrorFiltered_ + (1.0 - alpha) * dErrorRaw;
|
||||
dError = dErrorFiltered_;
|
||||
} else {
|
||||
dError = dErrorRaw;
|
||||
}
|
||||
|
||||
// ===== 3. 积分项 + 上下限 =====
|
||||
integralError_ += error * dt;
|
||||
|
||||
for (int i = 0; i < dof_; ++i) {
|
||||
double lower = (i < integralLowerLimit_.size()) ? integralLowerLimit_[i] : 1.0;
|
||||
double upper = (i < integralUpperLimit_.size()) ? integralUpperLimit_[i] : -1.0;
|
||||
|
||||
// upper <= lower (+EPS 容差) => 不启用积分限幅
|
||||
if (upper - lower > EPS) {
|
||||
if (integralError_[i] > upper) integralError_[i] = upper;
|
||||
if (integralError_[i] < lower) integralError_[i] = lower;
|
||||
}
|
||||
}
|
||||
|
||||
prevError_ = error;
|
||||
|
||||
// ===== 4. PID 输出（约定为关节力矩） =====
|
||||
Eigen::VectorXd tau =
|
||||
kp_.cwiseProduct(error) +
|
||||
ki_.cwiseProduct(integralError_) +
|
||||
kd_.cwiseProduct(dError);
|
||||
|
||||
return tau;
|
||||
}
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
@ -1,159 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 2025/8/24.
|
||||
//
|
||||
|
||||
#include "../include/touch_controller.h"
|
||||
|
||||
using namespace cmvr::ctrl;
|
||||
using namespace cmvr::msgs;
|
||||
|
||||
void TouchController::touch(std::shared_ptr<device::AbstractRobot> robot, const msgs::Pose3d pose, const msgs::Pose3d offset) {
|
||||
|
||||
msgs::Position target_position;
|
||||
target_position.set_x(pose.position().x() - offset.position().x());
|
||||
target_position.set_y(pose.position().y() - offset.position().y());
|
||||
target_position.set_z(pose.position().z() - offset.position().z());
|
||||
|
||||
|
||||
msgs::Euler target_euler;
|
||||
target_euler.set_rx(pose.euler().rx() - offset.euler().rx());
|
||||
target_euler.set_ry(pose.euler().ry() - offset.euler().ry());
|
||||
target_euler.set_rz(pose.euler().rz() - offset.euler().rz());
|
||||
|
||||
|
||||
msgs::Pose3d target_pose;
|
||||
*target_pose.mutable_position() = target_position;
|
||||
*target_pose.mutable_euler() = target_euler;
|
||||
|
||||
robot->moveJ("PELVIS_S","R_WRIST_R_S",target_pose);
|
||||
}
|
||||
|
||||
void TouchController::touch(msgs::Pose3d pose, msgs::Pose3d offset, double max_force) {
|
||||
auto hand_data = hand_->getSensorData();
|
||||
double force = 0;
|
||||
int x ,y;
|
||||
extractTouch(hand_data.index.tip.data,force,x,y);
|
||||
|
||||
double dz = pid_->compute(max_force, force, 0.01,10);
|
||||
|
||||
std::cout << dz << std::endl;
|
||||
|
||||
// LOG(INFO) << "Force : " << force << " dz : " << dz;
|
||||
|
||||
// // 输出 3x3 数组
|
||||
// LOG(INFO) << "Tip data (3x3):";
|
||||
// for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) {
|
||||
// std::stringstream ss;
|
||||
// for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) {
|
||||
// ss << hand_data.index.tip.data[i][j] << "\t";
|
||||
// }
|
||||
// LOG(INFO) << ss.str();
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool TouchController::extractTouch(const std::vector<std::vector<uint16_t> > &matrix, double &force, int &x, int &y) {
|
||||
|
||||
|
||||
int rows = matrix.size();
|
||||
int cols = matrix[0].size();
|
||||
double maxVal = 0;
|
||||
int maxX = -1, maxY = -1;
|
||||
double total = 0;
|
||||
|
||||
for (int i = 0; i < rows; i++) {
|
||||
for (int j = 0; j < cols; j++) {
|
||||
double val = matrix[i][j];
|
||||
total += val;
|
||||
if (val > maxVal) {
|
||||
maxVal = val;
|
||||
maxX = i;
|
||||
maxY = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (maxVal > touch_threshold_) {
|
||||
force = total;
|
||||
x = maxX;
|
||||
y = maxY;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool TouchController::isArrive(double max_force) {
|
||||
const auto& hand_data = hand_->getSensorData();
|
||||
|
||||
double force = 0.0;
|
||||
int x = 0, y = 0;
|
||||
extractTouch(hand_data.index.tip.data, force, x, y);
|
||||
|
||||
LOG(INFO) << "Force : " << force ;
|
||||
|
||||
// 输出 3x3 数组
|
||||
LOG(INFO) << "Tip data (3x3):";
|
||||
for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) {
|
||||
std::stringstream ss;
|
||||
for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) {
|
||||
ss << hand_data.index.tip.data[i][j] << "\t";
|
||||
}
|
||||
LOG(INFO) << ss.str();
|
||||
}
|
||||
|
||||
return force > max_force;
|
||||
}
|
||||
|
||||
void TouchController::touch(int u, int v, double max_force) {
|
||||
LOG(INFO) << "Touch request at pixel (" << u << ", " << v << ") with max_force=" << max_force;
|
||||
|
||||
// 获取目标点和当前位姿
|
||||
auto target_pose = cam_->get3DPointFromPixel(u, v);
|
||||
auto cur_pose = robot_->fk("PELVIS_S", "R_FINGER_TIP");
|
||||
|
||||
LOG(INFO) << "Target 3D Pose: " << target_pose;
|
||||
|
||||
// 定义关键点位
|
||||
auto pre_touch_pose = cur_pose;
|
||||
pre_touch_pose.mutable_position()->set_x(target_pose[0] - 0.05);
|
||||
pre_touch_pose.mutable_position()->set_y(target_pose[1]);
|
||||
pre_touch_pose.mutable_position()->set_z(target_pose[2]);
|
||||
|
||||
auto touch_pose = pre_touch_pose;
|
||||
touch_pose.mutable_position()->set_x(target_pose[0]);
|
||||
|
||||
const auto& retreat_pose = cur_pose;
|
||||
// retreat_pose.mutable_position()->set_x(target_pose[0] - 0.20);
|
||||
|
||||
// 1. 移动到预接触位置
|
||||
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", pre_touch_pose);
|
||||
|
||||
// 2. 向前接触并开启压力监测
|
||||
robot_->servoJ("PELVIS_S", "R_FINGER_TIP", touch_pose);
|
||||
|
||||
// 3. 等待达到最大压力
|
||||
constexpr int k_sleep_ms = 5;
|
||||
constexpr int k_timeout_ms = 5000;
|
||||
int elapsed_ms = 0;
|
||||
|
||||
while (!isArrive(max_force) && elapsed_ms < k_timeout_ms) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(k_sleep_ms));
|
||||
elapsed_ms += k_sleep_ms;
|
||||
}
|
||||
|
||||
if (elapsed_ms >= k_timeout_ms) {
|
||||
LOG(WARNING) << "Timeout waiting for force feedback!";
|
||||
} else {
|
||||
LOG(INFO) << "Max force reached, retreating.";
|
||||
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", retreat_pose);
|
||||
}
|
||||
}
|
||||
@ -1,71 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 2025/8/27.
|
||||
//
|
||||
|
||||
|
||||
#include "../../device_manager/include/device_manager.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "../include/touch_controller.h"
|
||||
#include "cmvr/msgs/geometry.pb.h"
|
||||
|
||||
|
||||
using namespace cmvr::device;
|
||||
using namespace cmvr::msgs;
|
||||
using namespace cmvr::ctrl;
|
||||
|
||||
TEST(TouchControllerTest,MyTest) {
|
||||
//
|
||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
||||
const XmlNode config(config_path);
|
||||
|
||||
if (!config.hasChild("DeviceManager")){
|
||||
LOG(ERROR) << "Device Manager node not found";
|
||||
}
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
|
||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
|
||||
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
|
||||
|
||||
cmvr::msgs::Pose3d pose;
|
||||
|
||||
pose.mutable_position()->set_x( 1.49969573e-01);
|
||||
pose.mutable_position()->set_y(-4.00100001e-01);
|
||||
pose.mutable_position()->set_z(-1.00102800e-01);
|
||||
|
||||
pose.mutable_euler()->set_rx(0);
|
||||
pose.mutable_euler()->set_ry(0);
|
||||
pose.mutable_euler()->set_rz(1.57);
|
||||
|
||||
robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
|
||||
// robot->seJ("PELVIS_S","R_WRIST_R_S",pose);
|
||||
// robot->servoJ("PELVIS_S","R_WRIST_R_S",pose,0.5);
|
||||
|
||||
cmvr::msgs::Pose3d delta_pose;
|
||||
delta_pose.mutable_position()->set_x( 0.01);
|
||||
delta_pose.mutable_position()->set_y(0);
|
||||
delta_pose.mutable_position()->set_z(0);
|
||||
|
||||
delta_pose.mutable_euler()->set_rx(0);
|
||||
delta_pose.mutable_euler()->set_ry(0);
|
||||
delta_pose.mutable_euler()->set_rz(0);
|
||||
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
||||
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02);
|
||||
|
||||
Pose3d offset;
|
||||
|
||||
TouchController controller(robot, hand,cam);
|
||||
|
||||
while (true) {
|
||||
// controller.touch(pose,offset,500);
|
||||
if (controller.isArrive(500)) {
|
||||
delta_pose.mutable_position()->set_x( -0.01);
|
||||
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.05);
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
}
|
||||
@ -2,28 +2,28 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE)
|
||||
find_package(osqp REQUIRED)
|
||||
find_package(OsqpEigen REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(fcl REQUIRED)
|
||||
pkg_check_modules(TINYXML2 REQUIRED tinyxml2)
|
||||
|
||||
include_directories(${TINYXML2_INCLUDE_DIRS})
|
||||
|
||||
add_library(ik_solver STATIC
|
||||
ik_solver/src/ik_solver.cpp
|
||||
ik_solver/src/ik_solver_creator.cpp
|
||||
opt_psi_limit_bias_solver/src/bias_srs_ik_slover.cpp
|
||||
opt_psi_limit_bias_solver/src/joints_limit_analyzer.cpp
|
||||
opt_psi_limit_bias_solver/src/opt_psi_limit_bias_solver.cpp
|
||||
opt_psi_limit_bias_solver/src/opt_psi_selector.cpp
|
||||
|
||||
add_library(ik_solver SHARED
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver_creator.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_dls_ik_solver.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_qp_ik_solver.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/bias_srs_ik_slover.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/joints_limit_analyzer.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/opt_psi_limit_bias_solver.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/opt_psi_selector.cpp
|
||||
)
|
||||
|
||||
target_include_directories(ik_solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(ik_solver PRIVATE
|
||||
target_link_libraries(ik_solver PUBLIC
|
||||
pinocchio_default
|
||||
pinocchio_parsers
|
||||
pinocchio_collision
|
||||
Eigen3::Eigen
|
||||
OsqpEigen::OsqpEigen
|
||||
${TINYXML2_LIBRARIES}
|
||||
fcl
|
||||
cmvr_es::data_center
|
||||
|
||||
)
|
||||
|
||||
add_library(cmvr_es::ik_solver ALIAS ik_solver)
|
||||
@ -34,18 +34,12 @@ add_library(cmvr_es::ik_solver ALIAS ik_solver)
|
||||
# --------------------------------------------------------
|
||||
find_package(glog REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(fcl REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
||||
)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/manif/0.0.5/include
|
||||
)
|
||||
|
||||
|
||||
link_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
||||
)
|
||||
@ -53,24 +47,43 @@ link_directories(
|
||||
|
||||
|
||||
|
||||
|
||||
add_executable(srs_ik_test
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/opt_psi_limit_bias_solver/src/srs_ik_test.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/srs_ik_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(srs_ik_test
|
||||
PRIVATE
|
||||
cmvr_es::mujoco_viewer
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog::glog
|
||||
proto-objects
|
||||
ccd
|
||||
fcl
|
||||
${OpenCV_LIBS}
|
||||
Eigen3::Eigen
|
||||
OsqpEigen::OsqpEigen
|
||||
cmvr_es::utils
|
||||
cmvr_es::data_center
|
||||
cmvr_es::ik_solver
|
||||
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
add_executable(ik_test
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/src/ik_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(ik_test
|
||||
PRIVATE
|
||||
cmvr_es::ik_solver
|
||||
cmvr_es::mujoco_viewer
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog::glog
|
||||
proto-objects
|
||||
|
||||
)
|
||||
|
||||
|
||||
@ -1,3 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 11/7/25.
|
||||
//
|
||||
@ -3,11 +3,11 @@
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <common/consts/constant.h>
|
||||
#include "ik_solver.h"
|
||||
#include <vector>
|
||||
#include <Eigen/Core>
|
||||
#include <common/consts/constant.h>
|
||||
|
||||
namespace cmvr {
|
||||
class IKSolver {
|
||||
public:
|
||||
@ -18,13 +18,14 @@ namespace cmvr {
|
||||
return false;
|
||||
}
|
||||
|
||||
virtual bool ik(const Eigen::Matrix4d &target_pose,std::vector<double> &joints_angle) {
|
||||
UNUSED_VARIABLE(target_pose,joints_angle);
|
||||
virtual bool ik(const Eigen::Matrix4d &target_pose,std::vector<double> &joints_angle,bool is_tcp = true) {
|
||||
UNUSED_VARIABLE(target_pose,joints_angle,is_tcp);
|
||||
return false;
|
||||
}
|
||||
|
||||
virtual bool fk(const std::vector<double> &joints_angle,Eigen::Matrix4d &cur_pose,bool robot_base = false) {
|
||||
UNUSED_VARIABLE(joints_angle,cur_pose,robot_base);
|
||||
// is_tcp = true ,末端是tcp .false ,末端是法兰
|
||||
virtual bool fk(const std::vector<double> &joints_angle,Eigen::Matrix4d &cur_pose,bool is_tcp = true) {
|
||||
UNUSED_VARIABLE(joints_angle,cur_pose,is_tcp);
|
||||
return false;
|
||||
};
|
||||
|
||||
@ -44,12 +45,15 @@ namespace cmvr {
|
||||
|
||||
|
||||
protected:
|
||||
//机械臂末端工具坐标系相对于末端法兰的变换矩阵
|
||||
//机械臂urdf中末端工具坐标系相对于MDH中末端法兰的变换矩阵
|
||||
Eigen::Matrix4d T_tool_flange_{};
|
||||
|
||||
// 机械臂基座相对于机器人基座的变换矩阵
|
||||
// 机械臂MDH基座相对于机器人urdf基座的变换矩阵
|
||||
Eigen::Matrix4d T_arm_robot_{};
|
||||
|
||||
// 机械臂urdf 中法兰姿态相对于 MDH 中法兰姿态的变换矩阵
|
||||
Eigen::Matrix4d T_flange_urdf_mdh_{};
|
||||
|
||||
// 上次调用ik 时计算的结果
|
||||
std::vector<double> cur_joints_angle_{};
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include "ik_solver/ik_solver/include/ik_solver.h"
|
||||
#include "ik_solver/include/ik_solver.h"
|
||||
|
||||
namespace cmvr
|
||||
{
|
||||
@ -16,6 +16,8 @@ namespace cmvr
|
||||
|
||||
// 最优臂角+角度限制+角度偏置的SRS的解析解
|
||||
OPT_PSI_LIMIT_BIAS_SRS = 1,
|
||||
PINOCCHIO_QP = 2,
|
||||
PINOCCHIO_DLS = 3
|
||||
};
|
||||
class IkSolverCreator
|
||||
{
|
||||
109
src/ik_solver/include/opt_psi_limit_bias_solver.h
Normal file
109
src/ik_solver/include/opt_psi_limit_bias_solver.h
Normal file
@ -0,0 +1,109 @@
|
||||
//
|
||||
// Created by lgv on 11/7/25.
|
||||
//
|
||||
#ifndef CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
|
||||
#define CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
||||
#include "ik_solver/include/bias_srs_ik_slover.h"
|
||||
#include "ik_solver/include/joints_limit_analyzer.h"
|
||||
#include "ik_solver/include/opt_psi_selector.h"
|
||||
#include "ik_solver/include/ik_solver.h"
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
class OptPsiLimitBiasSolver : public IKSolver {
|
||||
public:
|
||||
OptPsiLimitBiasSolver();
|
||||
~OptPsiLimitBiasSolver() override = default;
|
||||
|
||||
bool init() override;
|
||||
|
||||
bool ik(const Eigen::Matrix4d &target_pose,
|
||||
std::vector<double> &joints_angle,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
bool fk(const std::vector<double> &joints_angle,
|
||||
Eigen::Matrix4d &cur_pose,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
// === 配置 cost 参数 ===
|
||||
// cost = (Δψ)^2 + lambda_q_ * Σ(Δq_i)^2
|
||||
// 当前分支若 cost < cur_branch_cost_threshold_ 直接接受，不再搜索其它分支
|
||||
void set_cost_params(double lambda_q, double cur_branch_cost_threshold) {
|
||||
lambda_q_ = lambda_q;
|
||||
cur_branch_cost_threshold_ = cur_branch_cost_threshold;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<BiasSRSIkSolver> bias_srs_ik_solver_{nullptr};
|
||||
std::shared_ptr<JointsLimitAnalyzer> joints_limit_analyzer_{nullptr};
|
||||
std::shared_ptr<OptPsiSelector> opt_psi_selector_{nullptr};
|
||||
|
||||
// 维护“上一帧/当前”的臂角与分支状态
|
||||
struct BranchPsiState {
|
||||
bool valid{false};
|
||||
double psi{0.0}; // 上一次/当前的最优臂角 or 该分支的估计臂角
|
||||
int s_conf{+1}; // 肩分支: +1 对应 OUTWARD, -1 对应 INWARD
|
||||
int e_conf{+1}; // 肘分支
|
||||
int w_conf{+1}; // 腕分支
|
||||
};
|
||||
|
||||
// 当前选用的分支 + ψ
|
||||
BranchPsiState branch_state_;
|
||||
|
||||
// 每个分支在“当前关节下”的 ψ 估计（来自 estimate_state_from_current_joints）
|
||||
// 索引: [s_idx][e_idx][w_idx], 其中 idx = (sign >= 0 ? 0 : 1)
|
||||
BranchPsiState branch_init_[2][2][2];
|
||||
|
||||
// 代价参数：可以通过 set_cost_params 配置
|
||||
double lambda_q_{5.5}; // 关节变化权重
|
||||
double cur_branch_cost_threshold_{1e-3}; // 当前分支直接接受的 cost 阈值
|
||||
|
||||
// 从当前关节角反推出“当前 ψ + 分支 (s,e,w)”，同时填充 8 个分支的 branch_init_
|
||||
bool estimate_state_from_current_joints();
|
||||
|
||||
// 在给定分支上，利用 s_mat / w_mat 计算 ψ 可行域 + IK + cost
|
||||
bool solve_on_branch(const Eigen::Matrix4d &target_cal_pose,
|
||||
const Eigen::MatrixXd &s_mat,
|
||||
const Eigen::MatrixXd &w_mat,
|
||||
const BranchPsiState &branch,
|
||||
std::vector<double> &q_out,
|
||||
double &psi_out,
|
||||
double &cost_out);
|
||||
|
||||
// ConfigDirection ↔ 符号
|
||||
static inline int sign_from_dir(BiasSRSIkSolver::ConfigDirection d) {
|
||||
return (d == BiasSRSIkSolver::ConfigDirection::OUTWARD) ? +1 : -1;
|
||||
}
|
||||
|
||||
static inline BiasSRSIkSolver::ConfigDirection dir_from_sign(int s) {
|
||||
return (s >= 0)
|
||||
? BiasSRSIkSolver::ConfigDirection::OUTWARD
|
||||
: BiasSRSIkSolver::ConfigDirection::INWARD;
|
||||
}
|
||||
|
||||
// 符号 (-1/+1) → 索引 (1/0)
|
||||
static inline int idx_from_sign(int s) {
|
||||
return (s >= 0) ? 0 : 1;
|
||||
}
|
||||
|
||||
// 根据 (s,e,w) 取该分支的初始 ψ 状态（如果有）
|
||||
inline const BranchPsiState* get_branch_init_state(int s, int e, int w) const {
|
||||
int is = idx_from_sign(s);
|
||||
int ie = idx_from_sign(e);
|
||||
int iw = idx_from_sign(w);
|
||||
const BranchPsiState &st = branch_init_[is][ie][iw];
|
||||
return st.valid ? &st : nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
#endif // CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
|
||||
@ -37,5 +37,7 @@ namespace cmvr {
|
||||
double edge_margin_ = 1e-4; // 落到区间边界时的内缩量
|
||||
|
||||
static constexpr double EPS = 1e-9;
|
||||
|
||||
|
||||
};
|
||||
}
|
||||
105
src/ik_solver/include/pinocchio_dls_ik_solver.h
Normal file
105
src/ik_solver/include/pinocchio_dls_ik_solver.h
Normal file
@ -0,0 +1,105 @@
|
||||
//
|
||||
// Created by lgv on 11/28/25.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ik_solver/include/ik_solver.h"
|
||||
|
||||
#include <pinocchio/multibody/model.hpp>
|
||||
#include <pinocchio/multibody/data.hpp>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
class PinocchioDlsIKSolver : public IKSolver {
|
||||
public:
|
||||
/// urdf_path : 机械臂/整机 URDF 路径（可为双臂共基座整机）
|
||||
/// base_frame_name : 本 IK 链的基坐标系 frame 名（例如 "BASE" 或 "L_ARM_BASE"）
|
||||
/// flange_frame_name : 末端法兰 frame 名
|
||||
/// tcp_frame_name : 末端 TCP frame 名（可为空；为空则仅用法兰）
|
||||
PinocchioDlsIKSolver(const std::string &urdf_path,
|
||||
const std::string &base_frame_name,
|
||||
const std::string &flange_frame_name,
|
||||
const std::string &tcp_frame_name = std::string(),
|
||||
int max_iters = 100,
|
||||
double pos_eps = 1e-6,
|
||||
double rot_eps = 1e-6,
|
||||
double damping = 1e-6);
|
||||
|
||||
~PinocchioDlsIKSolver() override = default;
|
||||
|
||||
/// 加载 URDF，查找 base / flange / tcp frame，计算本链关节范围和限位
|
||||
bool init() override;
|
||||
|
||||
/// target_pose: URDF base 下的目标位姿
|
||||
/// is_tcp = true : 目标是 TCP 位姿
|
||||
/// is_tcp = false : 目标是法兰位姿
|
||||
///
|
||||
/// joints_angle:
|
||||
/// - 输出：长度 = 本链 DOF（chain_dof_）
|
||||
bool ik(const Eigen::Matrix4d &target_pose,
|
||||
std::vector<double> &joints_angle,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
/// joints_angle:
|
||||
/// - 若 size == chain_dof_ : 认为只传本臂关节
|
||||
/// - 若 size == model_.nq : 认为传整机关节
|
||||
/// cur_pose : URDF base 下当前 TCP/法兰位姿
|
||||
bool fk(const std::vector<double> &joints_angle,
|
||||
Eigen::Matrix4d &cur_pose,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
void setMaxIters(int iters) { max_iters_ = iters; }
|
||||
void setDamping(double d) { damping_ = d; }
|
||||
void setEps(double pos_eps, double rot_eps)
|
||||
{
|
||||
pos_eps_ = pos_eps;
|
||||
rot_eps_ = rot_eps;
|
||||
}
|
||||
|
||||
using IKSolver::setTcpTransform;
|
||||
using IKSolver::setArmBaseTransform;
|
||||
|
||||
private:
|
||||
static pinocchio::SE3 matrix4ToSE3(const Eigen::Matrix4d &T);
|
||||
static Eigen::Matrix4d se3ToMatrix4(const pinocchio::SE3 &M);
|
||||
Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda);
|
||||
|
||||
private:
|
||||
// 配置
|
||||
std::string urdf_path_;
|
||||
std::string base_frame_name_;
|
||||
std::string flange_frame_name_;
|
||||
std::string tcp_frame_name_;
|
||||
|
||||
// Pinocchio 模型
|
||||
pinocchio::Model model_;
|
||||
std::unique_ptr<pinocchio::Data> data_;
|
||||
|
||||
pinocchio::FrameIndex base_frame_id_{(pinocchio::FrameIndex)(-1)};
|
||||
pinocchio::FrameIndex flange_frame_id_{(pinocchio::FrameIndex)(-1)};
|
||||
pinocchio::FrameIndex tcp_frame_id_{(pinocchio::FrameIndex)(-1)};
|
||||
bool has_tcp_{false};
|
||||
|
||||
// 本链在整机 q 向量中的起始 index 和 DOF 数
|
||||
int chain_q_start_{0};
|
||||
int chain_dof_{0};
|
||||
|
||||
// 本链的关节限位（从 Pinocchio 模型中截取）
|
||||
Eigen::VectorXd q_lower_chain_;
|
||||
Eigen::VectorXd q_upper_chain_;
|
||||
|
||||
bool initialized_{false};
|
||||
|
||||
int max_iters_;
|
||||
double pos_eps_;
|
||||
double rot_eps_;
|
||||
double damping_;
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
130
src/ik_solver/include/pinocchio_qp_ik_solver.h
Normal file
130
src/ik_solver/include/pinocchio_qp_ik_solver.h
Normal file
@ -0,0 +1,130 @@
|
||||
//
|
||||
// Created by lgv on 12/2/25.
|
||||
//
|
||||
|
||||
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ik_solver/include/ik_solver.h"
|
||||
#include "common/utils/math/qp_solver.h"
|
||||
|
||||
#include <pinocchio/multibody/model.hpp>
|
||||
#include <pinocchio/multibody/data.hpp>
|
||||
#include <pinocchio/spatial/se3.hpp>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
class PinocchioQpIKSolver : public IKSolver {
|
||||
public:
|
||||
/// urdf_path : URDF 路径（可以是单臂，也可以是双臂整机）
|
||||
/// base_frame_name : 作为 IK 基坐标系的 frame 名（例：PELVIS_S）
|
||||
/// flange_frame_name : 末端法兰 frame 名（例：L_FLANGE）
|
||||
/// tcp_frame_name : TCP frame 名（可为空；为空则 is_tcp=true 时也用法兰）
|
||||
/// lambda : 阻尼（cost 中的 √lambda * I）
|
||||
/// w_posrot : 位置 / 姿态误差权重（0~1，越大越重姿态）
|
||||
/// max_iters : 外层迭代上限
|
||||
/// tol : 误差收敛阈值（||任务误差||）
|
||||
/// qp_time_limit : 每次 QP 的时间限制（秒）
|
||||
PinocchioQpIKSolver(const std::string &urdf_path,
|
||||
const std::string &base_frame_name,
|
||||
const std::string &flange_frame_name,
|
||||
const std::string &tcp_frame_name = std::string(),
|
||||
double lambda = 1e-4,
|
||||
double w_posrot = 0.5,
|
||||
int max_iters = 100,
|
||||
double tol = 1e-6,
|
||||
double qp_time_limit = 1e-2);
|
||||
|
||||
~PinocchioQpIKSolver() override = default;
|
||||
|
||||
/// 加载 URDF、查找 base / flange / tcp frame，并构建 base→flange 子链
|
||||
bool init() override;
|
||||
|
||||
/// target_pose : URDF base 下的目标位姿（4x4）
|
||||
/// joints_angle:
|
||||
/// - 输出：长度 = 子链 DOF（base→flange 的关节数）
|
||||
/// is_tcp = true : 目标是 TCP 位姿（若有 tcp frame，否则 fallback 到 flange）
|
||||
/// is_tcp = false : 目标是法兰位姿
|
||||
bool ik(const Eigen::Matrix4d &target_pose,
|
||||
std::vector<double> &joints_angle,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
/// joints_angle : 当前子链关节角（长度 = 子链 DOF）
|
||||
/// cur_pose : URDF base 下当前 TCP / 法兰位姿
|
||||
bool fk(const std::vector<double> &joints_angle,
|
||||
Eigen::Matrix4d &cur_pose,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
/// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
|
||||
void setVelocityLimits(const Eigen::VectorXd &qd_max);
|
||||
void setAccelerationLimits(const Eigen::VectorXd &qdd_max);
|
||||
|
||||
using IKSolver::update_joints_state;
|
||||
using IKSolver::setTcpTransform;
|
||||
using IKSolver::setArmBaseTransform;
|
||||
|
||||
private:
|
||||
static pinocchio::SE3 matrix4ToSE3(const Eigen::Matrix4d &T);
|
||||
static Eigen::Matrix4d se3ToMatrix4(const pinocchio::SE3 &M);
|
||||
|
||||
/// 根据 base_frame_name_ 和 flange_frame_name_ 构建 base→flange 子链
|
||||
/// 并建立：
|
||||
/// active_joints_ : 子链上的 joint index
|
||||
/// active_q_idx_ : 对应在 q_full_ 里的索引
|
||||
/// active_v_idx_ : 对应在 v 里的索引（做雅可比抽列用）
|
||||
/// q_min/q_max/... : 子链限位
|
||||
bool buildActiveChain();
|
||||
|
||||
private:
|
||||
// 配置
|
||||
std::string urdf_path_;
|
||||
std::string base_frame_name_;
|
||||
std::string flange_frame_name_;
|
||||
std::string tcp_frame_name_;
|
||||
|
||||
double lambda_;
|
||||
double w_posrot_;
|
||||
int max_iters_;
|
||||
double tol_;
|
||||
double qp_time_limit_;
|
||||
|
||||
// Pinocchio 模型
|
||||
pinocchio::Model model_;
|
||||
std::unique_ptr<pinocchio::Data> data_;
|
||||
|
||||
int full_dof_{0}; // 整机 nq
|
||||
int dof_{0}; // 子链 DOF（只在这条链上做 IK）
|
||||
|
||||
pinocchio::FrameIndex base_frame_id_{(pinocchio::FrameIndex)(-1)};
|
||||
pinocchio::FrameIndex flange_frame_id_{(pinocchio::FrameIndex)(-1)};
|
||||
pinocchio::FrameIndex tcp_frame_id_{(pinocchio::FrameIndex)(-1)};
|
||||
bool has_tcp_{false};
|
||||
|
||||
// base→flange 子链（只支持每个关节 nq=1,nv=1 的情况）
|
||||
std::vector<pinocchio::JointIndex> active_joints_;
|
||||
std::vector<int> active_q_idx_; // 子链每个关节在 q_full_ (nq) 中的索引
|
||||
std::vector<int> active_v_idx_; // 子链每个关节在 v (nv) 中的索引
|
||||
|
||||
// 子链上的限位
|
||||
Eigen::VectorXd q_min_global_;
|
||||
Eigen::VectorXd q_max_global_;
|
||||
Eigen::VectorXd qd_max_global_;
|
||||
Eigen::VectorXd qdd_max_global_;
|
||||
|
||||
bool initialized_{false};
|
||||
|
||||
// 整机 q（长度 = full_dof_），用于 Pinocchio 正解 / 雅可比
|
||||
Eigen::VectorXd q_full_;
|
||||
|
||||
// QP 求解器（OsqpEigen 封装）
|
||||
QPSolver solver_;
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
@ -1,37 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 11/7/25.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
|
||||
#define CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
|
||||
#include "joints_limit_analyzer.h"
|
||||
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/joints_limit_analyzer.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_selector.h"
|
||||
#include "ik_solver/ik_solver/include/ik_solver.h"
|
||||
|
||||
namespace cmvr{
|
||||
class OptPsiLimitBiasSolver : public IKSolver{
|
||||
public:
|
||||
OptPsiLimitBiasSolver();
|
||||
~OptPsiLimitBiasSolver()=default;
|
||||
|
||||
bool init() override;
|
||||
bool ik(const Eigen::Matrix4d &target_pose, std::vector<double> &joints_angle) override;
|
||||
bool fk(const std::vector<double> &joints_angle, Eigen::Matrix4d &cur_pose, bool robot_base = false) override;
|
||||
|
||||
private:
|
||||
std::shared_ptr<BiasSRSIkSolver> bias_srs_ik_solver_{nullptr};
|
||||
std::shared_ptr<JointsLimitAnalyzer> joints_limit_analyzer_{nullptr};
|
||||
std::shared_ptr<OptPsiSelector> opt_psi_selector_{nullptr};
|
||||
// 先前的最优臂角
|
||||
struct {
|
||||
double value{};
|
||||
bool valid{false};
|
||||
}prev_opt_psi_;
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
#endif //CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
|
||||
@ -1,114 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 11/7/25.
|
||||
//
|
||||
|
||||
#include "opt_psi_limit_bias_solver/include/opt_psi_limit_bias_solver.h"
|
||||
|
||||
using namespace cmvr;
|
||||
|
||||
OptPsiLimitBiasSolver::OptPsiLimitBiasSolver() : IKSolver(){
|
||||
bias_srs_ik_solver_ = std::make_shared<BiasSRSIkSolver>();
|
||||
joints_limit_analyzer_ = std::make_shared<JointsLimitAnalyzer>();
|
||||
opt_psi_selector_ = std::make_shared<OptPsiSelector>();
|
||||
|
||||
this->init();
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::init() {
|
||||
Eigen::Matrix4d T_tool_flange, T_arm_robot;
|
||||
T_tool_flange << 0, 1, 0, -0.284077,
|
||||
0, 0, 1, 0.00801525,
|
||||
1, 0, 0, 0.00684256,
|
||||
0, 0, 0, 1;
|
||||
|
||||
|
||||
T_arm_robot << 0, 1, 0, 0,
|
||||
0, 0, -1, 0,
|
||||
-1, 0, 0, 0.042,
|
||||
0, 0, 0, 1;
|
||||
|
||||
setTcpTransform(T_tool_flange);
|
||||
setArmBaseTransform(T_arm_robot);
|
||||
opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose, std::vector<double> &joints_angle) {
|
||||
Eigen::Matrix4d target_cal_pose = SupportFunctions::invertHomogeneous(T_arm_robot_) * target_pose * SupportFunctions::invertHomogeneous(T_tool_flange_);
|
||||
// 0 : 先验证一下 保存的最优臂角是否有问题
|
||||
if (prev_opt_psi_.valid == true) {
|
||||
auto cur_joints_angle = bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, prev_opt_psi_.value);
|
||||
for (int i = 0; i < cur_joints_angle_.size(); i++) {
|
||||
if (std::abs(cur_joints_angle[i] - cur_joints_angle_[i]) > 1e-3) {
|
||||
prev_opt_psi_.valid = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 1：根据当前位置预估当前位置的最优臂角
|
||||
if (prev_opt_psi_.valid == false) {
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
Eigen::Matrix4d cur_pose;
|
||||
fk(cur_joints_angle_, cur_pose);
|
||||
bias_srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat);
|
||||
|
||||
auto s = bias_srs_ik_solver_->get_shoulder_config();
|
||||
auto e = bias_srs_ik_solver_->get_elbow_config();
|
||||
auto w = bias_srs_ik_solver_->get_wrist_config();
|
||||
|
||||
auto res = joints_limit_analyzer_->estimate_psi_from_joints(s_mat, w_mat,
|
||||
cur_joints_angle_, s, e, w);
|
||||
|
||||
if (res.ok) {
|
||||
prev_opt_psi_.valid = true;
|
||||
prev_opt_psi_.value = res.psi;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 2：求取臂角范围
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
bias_srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat);
|
||||
|
||||
auto s = bias_srs_ik_solver_->get_shoulder_config();
|
||||
auto e = bias_srs_ik_solver_->get_elbow_config();
|
||||
auto w = bias_srs_ik_solver_->get_wrist_config();
|
||||
auto joints_limits = bias_srs_ik_solver_->get_joints_limits();
|
||||
|
||||
auto limits = joints_limit_analyzer_->calc_arm_angle_limits(s_mat, w_mat,
|
||||
joints_limits, s, e, w);
|
||||
|
||||
// 3： 求取当前最优臂角
|
||||
double best_psi{};
|
||||
if (prev_opt_psi_.valid == false) {
|
||||
return false;
|
||||
} else {
|
||||
opt_psi_selector_->update_psi(best_psi, prev_opt_psi_.value, limits);
|
||||
prev_opt_psi_.value = best_psi;
|
||||
prev_opt_psi_.valid = true;
|
||||
}
|
||||
|
||||
// 4 : 求取关节角
|
||||
joints_angle = bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, best_psi);
|
||||
|
||||
// 5： 验证有没有超出限制
|
||||
for (int i = 0; i < joints_angle.size(); ++i) {
|
||||
if ((joints_angle[i] > joints_limits[i].second) || (joints_angle[i] < joints_limits[i].first)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool OptPsiLimitBiasSolver::fk(const std::vector<double> &joints_angle, Eigen::Matrix4d &cur_pose, bool robot_base) {
|
||||
|
||||
cur_pose = bias_srs_ik_solver_->calc_total_transform(joints_angle);
|
||||
if (robot_base == true) {
|
||||
cur_pose = T_arm_robot_ * cur_pose *T_tool_flange_;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -1,385 +0,0 @@
|
||||
//
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/joints_limit_analyzer.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_selector.h"
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <fstream>
|
||||
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_limit_bias_solver.h"
|
||||
|
||||
using namespace cmvr;
|
||||
|
||||
|
||||
struct IkSample {
|
||||
double psi;
|
||||
std::array<double, 7> q; // q1..q7
|
||||
};
|
||||
|
||||
|
||||
bool write_ik_samples_csv(const std::string &filepath,
|
||||
const std::vector<IkSample> &samples,
|
||||
bool write_header,
|
||||
int precision) {
|
||||
std::ofstream ofs(filepath, std::ios::out | std::ios::trunc);
|
||||
if (!ofs.is_open()) return false;
|
||||
|
||||
// 固定小数点（避免本地化成逗号）
|
||||
ofs.imbue(std::locale::classic());
|
||||
ofs << std::fixed << std::setprecision(precision);
|
||||
|
||||
if (write_header) {
|
||||
ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n";
|
||||
}
|
||||
for (const auto &s: samples) {
|
||||
ofs << s.psi;
|
||||
for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i];
|
||||
ofs << '\n';
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
// std::cout << std::fixed << std::setprecision(7);
|
||||
|
||||
BiasSRSIkSolver slover;
|
||||
std::vector<IkSample> samples;
|
||||
samples.reserve(4096);
|
||||
|
||||
std::vector<double> joint_angles(7, 0);
|
||||
joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56};
|
||||
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||
|
||||
|
||||
// 目标位姿：FK(joint_angles)
|
||||
const auto target_pose = slover.calc_total_transform(joint_angles);
|
||||
cout << "Target Pose (FK from seed joints):\n" << target_pose << endl;
|
||||
|
||||
// 系数矩阵 & ψ 扫描区间
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
|
||||
|
||||
auto res = JointsLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
|
||||
slover.get_elbow_config(), slover.get_wrist_config());
|
||||
|
||||
if (res.ok) {
|
||||
std::cout << "res.psi" << res.psi << std::endl;
|
||||
}
|
||||
auto limits = JointsLimitAnalyzer::calc_arm_angle_limits(s_mat, w_mat, slover.get_joints_limits(),
|
||||
slover.get_shoulder_config(),
|
||||
slover.get_elbow_config(), slover.get_wrist_config());
|
||||
|
||||
// 误差统计
|
||||
const double kPosTol = 1e-4; // 位置容差（m）
|
||||
const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573°
|
||||
double max_pos_err = 0.0, max_rot_err = 0.0;
|
||||
double sum_pos_err = 0.0, sum_rot_err = 0.0;
|
||||
size_t total = 0, bad = 0;
|
||||
|
||||
// 便捷引用
|
||||
const Eigen::Vector3d p_target = target_pose.block<3, 1>(0, 3);
|
||||
const Eigen::Matrix3d R_target = target_pose.block<3, 3>(0, 0);
|
||||
|
||||
auto clamp = [](double x, double lo, double hi) {
|
||||
return std::max(lo, std::min(hi, x));
|
||||
};
|
||||
|
||||
auto rot_err_rad = [&](const Eigen::Matrix3d &R) -> double {
|
||||
Eigen::Matrix3d dR = R_target.transpose() * R;
|
||||
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
||||
return std::acos(c); // [0, pi]
|
||||
};
|
||||
|
||||
// 表头
|
||||
cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||
|
||||
// ψ 扫描
|
||||
for (const auto &limit: limits) {
|
||||
const double psi_lo = limit.first;
|
||||
const double psi_hi = limit.second;
|
||||
|
||||
for (double psi = psi_lo; psi < psi_hi; psi += 0.001) {
|
||||
// IK 解
|
||||
auto q = slover.inverse_kinematics(target_pose, psi);
|
||||
if (q.size() != 7 || std::any_of(q.begin(), q.end(),
|
||||
[](double v) { return !std::isfinite(v); })) {
|
||||
++bad;
|
||||
++total;
|
||||
cout << psi << ", nan, nan, nan\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
// 用 IK 解做 FK，计算误差
|
||||
const auto T_fk = slover.calc_total_transform(q);
|
||||
const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3);
|
||||
const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0);
|
||||
|
||||
const double pos_err = (p_fk - p_target).norm();
|
||||
const double rot_err = rot_err_rad(R_fk);
|
||||
const double rot_err_deg = rot_err * 180.0 / M_PI;
|
||||
|
||||
// 打印每个样本的误差（一行，便于看 sweep）
|
||||
cout << psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
|
||||
|
||||
// 断言（不立即中止）
|
||||
SCOPED_TRACE(testing::Message() << "psi=" << psi);
|
||||
EXPECT_LT(pos_err, kPosTol);
|
||||
EXPECT_LT(rot_err, kRotTol);
|
||||
|
||||
// 统计
|
||||
max_pos_err = std::max(max_pos_err, pos_err);
|
||||
max_rot_err = std::max(max_rot_err, rot_err);
|
||||
sum_pos_err += pos_err;
|
||||
sum_rot_err += rot_err;
|
||||
++total;
|
||||
|
||||
// 保留你原先的采样输出
|
||||
samples.push_back(IkSample{psi, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
|
||||
}
|
||||
}
|
||||
|
||||
// 摘要打印
|
||||
cout << "\nSummary:\n"
|
||||
<< " total=" << total
|
||||
<< " bad=" << bad
|
||||
<< " pos_err_max=" << max_pos_err << " m"
|
||||
<< " rot_err_max=" << max_rot_err << " rad (" << max_rot_err * 180.0 / M_PI << " deg)\n"
|
||||
<< " pos_err_mean=" << (total ? (sum_pos_err / total) : 0.0) << " m"
|
||||
<< " rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad ("
|
||||
<< (total ? (sum_rot_err / total) * 180.0 / M_PI : 0.0) << " deg)\n";
|
||||
|
||||
// 文件输出（与原逻辑一致）
|
||||
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
|
||||
|
||||
// 最终强约束（避免全是 EXPECT_* 时忽略失败）
|
||||
ASSERT_LT(max_pos_err, 10 * kPosTol) << "Max position error too large.";
|
||||
ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large.";
|
||||
}
|
||||
|
||||
|
||||
TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
// std::cout << std::fixed << std::setprecision(7);
|
||||
|
||||
BiasSRSIkSolver slover;
|
||||
std::vector<IkSample> samples;
|
||||
samples.reserve(4096);
|
||||
|
||||
// 1: 当前位姿
|
||||
std::vector<double> joint_angles(7, 0);
|
||||
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||
const auto cur_pose = slover.calc_total_transform(joint_angles);
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
slover.cal_coefficient_matrix(cur_pose, s_mat, w_mat);
|
||||
auto res = JointsLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
|
||||
slover.get_elbow_config(), slover.get_wrist_config());
|
||||
|
||||
// 2: 目标位姿
|
||||
joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56};
|
||||
const auto target_pose = slover.calc_total_transform(joint_angles);
|
||||
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
|
||||
|
||||
// 3； 计算limit
|
||||
auto limits = JointsLimitAnalyzer::calc_arm_angle_limits(s_mat, w_mat, slover.get_joints_limits(),
|
||||
slover.get_shoulder_config(),
|
||||
slover.get_elbow_config(), slover.get_wrist_config());
|
||||
|
||||
// 4； 计算best
|
||||
OptPsiSelector opt_psi_selector;
|
||||
double best_psi{};
|
||||
opt_psi_selector.update_psi(best_psi,res.psi, limits);
|
||||
|
||||
// 误差统计
|
||||
const double kPosTol = 1e-4; // 位置容差（m）
|
||||
const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573°
|
||||
double max_pos_err = 0.0, max_rot_err = 0.0;
|
||||
double sum_pos_err = 0.0, sum_rot_err = 0.0;
|
||||
size_t total = 0, bad = 0;
|
||||
|
||||
// 便捷引用
|
||||
const Eigen::Vector3d p_target = target_pose.block<3, 1>(0, 3);
|
||||
const Eigen::Matrix3d R_target = target_pose.block<3, 3>(0, 0);
|
||||
|
||||
auto clamp = [](double x, double lo, double hi) {
|
||||
return std::max(lo, std::min(hi, x));
|
||||
};
|
||||
|
||||
auto rot_err_rad = [&](const Eigen::Matrix3d &R) -> double {
|
||||
Eigen::Matrix3d dR = R_target.transpose() * R;
|
||||
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
||||
return std::acos(c); // [0, pi]
|
||||
};
|
||||
|
||||
// IK 解
|
||||
auto q = slover.inverse_kinematics(target_pose, best_psi);
|
||||
|
||||
for (double q1: q) {
|
||||
std::cout << q1 << " , ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
// 用 IK 解做 FK，计算误差
|
||||
const auto T_fk = slover.calc_total_transform(q);
|
||||
const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3);
|
||||
const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0);
|
||||
|
||||
const double pos_err = (p_fk - p_target).norm();
|
||||
const double rot_err = rot_err_rad(R_fk);
|
||||
const double rot_err_deg = rot_err * 180.0 / M_PI;
|
||||
|
||||
|
||||
// 表头
|
||||
cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||
cout << best_psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
|
||||
}
|
||||
|
||||
|
||||
TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) {
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
|
||||
std::vector<IkSample> samples;
|
||||
samples.reserve(4096);
|
||||
|
||||
OptPsiLimitBiasSolver solver;
|
||||
|
||||
|
||||
// 1) 当前位姿（估计上一时刻 ψ 用）
|
||||
std::vector<double> joint_angles(7, 0);
|
||||
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||
solver.update_joints_state(joint_angles);
|
||||
samples.push_back(IkSample{
|
||||
0.0, {
|
||||
joint_angles[0], joint_angles[1],
|
||||
joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6]
|
||||
}
|
||||
});
|
||||
|
||||
// 2) 目标位姿（作为直线的起点）
|
||||
joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 0.29, 0.59};
|
||||
Eigen::Matrix4d target_pose;
|
||||
|
||||
solver.fk(joint_angles,target_pose,true);
|
||||
|
||||
// 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点）
|
||||
const int N = 100; // 采样点数（间隔均匀）
|
||||
const double L = 0.20; // 直线长度 0.20 m
|
||||
Eigen::Vector3d dir = Eigen::Vector3d::UnitX();
|
||||
dir.normalize();
|
||||
|
||||
// 固定姿态（也可以改成对姿态做 Slerp）
|
||||
const Eigen::Matrix3d R_fixed = target_pose.block < 3,
|
||||
3 > (0, 0);
|
||||
const Eigen::Vector3d p0 = target_pose.block < 3,
|
||||
1 > (0, 3);
|
||||
|
||||
// 3) 初始 ψ：用估计得到的 ψ，再根据 target_pose 的可行区间做一次更新
|
||||
std::vector<double> q;
|
||||
solver.ik(target_pose,q);
|
||||
|
||||
// 4) 误差评估工具
|
||||
const auto clamp = [](double x, double lo, double hi) {
|
||||
return std::max(lo, std::min(hi, x));
|
||||
};
|
||||
auto rot_err_rad = [&](const Eigen::Matrix3d &R_goal, const Eigen::Matrix3d &R_fk) -> double {
|
||||
Eigen::Matrix3d dR = R_goal.transpose() * R_fk;
|
||||
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
||||
return std::acos(c);
|
||||
};
|
||||
|
||||
samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
|
||||
cout << "idx, s(0..1), psi(rad), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||
|
||||
// 5) 直线采样 & 每点求 IK（带 ψ 更新）
|
||||
for (int k = 0; k <= N; ++k) {
|
||||
const double s = static_cast<double>(k) / static_cast<double>(N); // [0,1]
|
||||
Eigen::Vector3d p = p0 + s * L * dir;
|
||||
|
||||
Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity();
|
||||
T_goal.block<3, 3>(0, 0) = R_fixed;
|
||||
T_goal.block<3, 1>(0, 3) = p;
|
||||
|
||||
// 计算当前点的 arm-angle 可行区间，并基于上一时刻 psi_curr 更新一次
|
||||
|
||||
|
||||
// 逆解（带 ψ）
|
||||
solver.ik(T_goal,q);
|
||||
solver.update_joints_state(q);
|
||||
|
||||
// 容错：若 IK 失败（大小不为 7），跳过但打印提示
|
||||
if (q.size() != 7) {
|
||||
cout << k << ", " << s << ", " << 0.0
|
||||
<< ", IK_FAIL, , , , , , , , ,\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
// 前向校验
|
||||
Eigen::Matrix4d T_fk;
|
||||
solver.fk(q,T_fk,true);
|
||||
const Eigen::Vector3d p_fk = T_fk.block < 3,
|
||||
1 > (0, 3);
|
||||
const Eigen::Matrix3d R_fk = T_fk.block < 3,
|
||||
3 > (0, 0);
|
||||
|
||||
const double pos_err = (p_fk - p).norm();
|
||||
const double rot_err = rot_err_rad(R_fixed, R_fk);
|
||||
const double rot_err_deg = rot_err * 180.0 / M_PI;
|
||||
|
||||
cout << k << ", " << s << ", " << 0.0 << ", "
|
||||
<< q[0] << ", " << q[1] << ", " << q[2] << ", "
|
||||
<< q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", "
|
||||
<< pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
|
||||
|
||||
samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
|
||||
}
|
||||
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
|
||||
}
|
||||
|
||||
|
||||
TEST(SRS_IK_TEST, TR_TEST) {
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
|
||||
OptPsiLimitBiasSolver solver;
|
||||
Eigen::Matrix4d T;
|
||||
T << 9.99998311e-01, -1.78940420e-03, 4.20611000e-04, 3.83367005e-02,
|
||||
6.06804000e-05, -1.96560031e-01, -9.80491790e-01, -1.08356411e-01,
|
||||
1.83717140e-03, 9.80490159e-01, -1.96559590e-01, -7.19901808e-01,
|
||||
0.0, 0.0, 0.0, 1.0;
|
||||
|
||||
std::vector<double> joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||
std::vector<double> last_joint_angles = {0.00203898, 1.34062, 0.111, 0.622261, 0.256, -0.0210733, 1.00};
|
||||
|
||||
|
||||
|
||||
// 目标位姿：FK(joint_angles)
|
||||
Eigen::Matrix4d pose;
|
||||
solver.fk(joint_angles,pose,true);
|
||||
cout << "Target Pose (FK from seed joints):\n" << pose << endl;
|
||||
|
||||
|
||||
|
||||
solver.update_joints_state(last_joint_angles);
|
||||
solver.ik(pose,joint_angles);
|
||||
|
||||
for (double joint_angle: joint_angles) {
|
||||
cout << joint_angle << endl;
|
||||
}
|
||||
|
||||
solver.fk(joint_angles,pose,true);
|
||||
cout << "Target Pose (FK from seed joints):\n" << pose << endl;
|
||||
|
||||
|
||||
}
|
||||
@ -2,9 +2,10 @@
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
#include "opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h"
|
||||
#include "ik_solver/include/bias_srs_ik_slover.h"
|
||||
#include <iostream>
|
||||
#include "common/utils/math/support_functions.h"
|
||||
#include "common/consts/constant.h"
|
||||
using namespace cmvr;
|
||||
using namespace Eigen;
|
||||
|
||||
@ -28,15 +29,19 @@ BiasSRSIkSolver::BiasSRSIkSolver() {
|
||||
d_ew_ = link_lengths_[2];
|
||||
d_wt_ = link_lengths_[3];
|
||||
|
||||
|
||||
|
||||
|
||||
joints_limits_ = {
|
||||
{-0.26, 1.57},
|
||||
{-M_PI, M_PI},
|
||||
{-0.78, 1.57},
|
||||
{-M_PI, M_PI},
|
||||
{0, 2.05},
|
||||
{-3.00, 3.0},
|
||||
{-2, 2},
|
||||
{-0.57, 1.57},
|
||||
{-M_PI, M_PI},
|
||||
{-0.78, 0.78},
|
||||
{-0.26, 1.57},
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
Eigen::Matrix3d BiasSRSIkSolver::reference_plane(const Eigen::Vector3d &S, const Eigen::Vector3d &W) {
|
||||
@ -110,13 +115,23 @@ std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &p
|
||||
|
||||
|
||||
double d_sw = (W - S).norm();
|
||||
if ((std::abs(d_se_ + d_ew_) < (d_sw - 1e-6) || (d_sw + 1e-6) < std::abs(d_se_ - d_ew_))) {
|
||||
const double r_max = d_se_ + d_ew_; // 最远：完全伸直
|
||||
const double r_min = std::abs(d_se_ - d_ew_); // 最近：完全折叠
|
||||
|
||||
// 与上下界的偏差
|
||||
double diff_max = d_sw - r_max; // >0 表示比 r_max 还远
|
||||
double diff_min = r_min - d_sw; // >0 表示比 r_min 还近
|
||||
|
||||
// 明显超过可达范围才判定为不可达：
|
||||
// - diff_max > EPS → 太远
|
||||
// - diff_min > EPS → 太近
|
||||
if (diff_max > EPS || diff_min > EPS) {
|
||||
throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
|
||||
}
|
||||
|
||||
// 计算肘部角度 关节3
|
||||
double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2 * d_se_ * d_ew_);
|
||||
cos_elbow = std::clamp(double(cos_elbow), -1.0, 1.0);
|
||||
cos_elbow = std::clamp(cos_elbow, -1.0, 1.0);
|
||||
joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow));
|
||||
|
||||
// 计算臂平面E处相对于 基坐标系 的旋转矩阵
|
||||
@ -126,9 +141,33 @@ std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &p
|
||||
|
||||
|
||||
// 计算肩部角度 (关节0, 1, 2)
|
||||
joints[0] = std::atan2(-R3(1, 1) * shoulder_config_, -R3(0, 1) * shoulder_config_);
|
||||
joints[1] = std::acos(std::clamp(double(-R3(2, 1)), -0.9999999999, 0.999999999)) * shoulder_config_;
|
||||
joints[2] = std::atan2(R3(2, 2) * shoulder_config_, -R3(2, 0) * shoulder_config_);
|
||||
// joints[0] = std::atan2(-R3(1, 1) * shoulder_config_, -R3(0, 1) * shoulder_config_);
|
||||
// joints[1] = std::acos(std::clamp(-R3(2, 1), -1.0, 1.0)) * shoulder_config_;
|
||||
// joints[2] = std::atan2(R3(2, 2) * shoulder_config_, -R3(2, 0) * shoulder_config_);
|
||||
|
||||
double k = shoulder_config_;
|
||||
|
||||
// c2 = np.clip(-R3[2, 1], -1.0, 1.0)
|
||||
double c2 = std::clamp(-R3(2, 1), -1.0, 1.0); // cos(theta2)
|
||||
const double eps = 1e-8;
|
||||
|
||||
if (std::fabs(c2 - 1.0) < eps) {
|
||||
// ===== theta2 ≈ 0 奇异 =====
|
||||
joints[1] = 0.0 * k;
|
||||
joints[0] = 0.0;
|
||||
joints[2] = std::atan2(k * R3(1, 0), k * R3(0, 0));
|
||||
} else if (std::fabs(c2 + 1.0) < eps) {
|
||||
// ===== theta2 ≈ π 或 -π 奇异 =====
|
||||
joints[1] = k * M_PI; //
|
||||
joints[2] = std::atan2(k * R3(1, 0), k * R3(1, 2));
|
||||
joints[0] = 0.0;
|
||||
} else {
|
||||
// ===== 一般情况 =====
|
||||
joints[0] = std::atan2(-k * R3(1, 1), -k * R3(0, 1));
|
||||
joints[1] = k * std::acos(c2);
|
||||
joints[2] = std::atan2(k * R3(2, 2), -k * R3(2, 0));
|
||||
}
|
||||
|
||||
|
||||
// 计算 R04
|
||||
Eigen::Matrix3d R04 = Eigen::Matrix3d::Identity();
|
||||
@ -143,27 +182,53 @@ std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &p
|
||||
|
||||
|
||||
// 提取腕部欧拉角
|
||||
double phi_z = std::atan2(R47(1, 2), R47(0, 2));
|
||||
double theta_y = std::atan2(std::sqrt(R47(2, 0) * R47(2, 0) + R47(2, 1) * R47(2, 1)), R47(2, 2));
|
||||
double psi_z = std::atan2(R47(2, 1), -R47(2, 0));
|
||||
// double phi_z = std::atan2(R47(1, 2), R47(0, 2));
|
||||
// double theta_y = std::atan2(std::sqrt(R47(2, 0) * R47(2, 0) + R47(2, 1) * R47(2, 1)), R47(2, 2));
|
||||
// double psi_z = std::atan2(R47(2, 1), -R47(2, 0));
|
||||
//
|
||||
// // 处理奇异情况
|
||||
// if (std::sin(theta_y) < 1e-12) {
|
||||
// phi_z = std::atan2(R47(1, 0), R47(0, 0));
|
||||
// psi_z = 0.0;
|
||||
// }
|
||||
// if (std::sin(M_PI - theta_y) < 1e-12) {
|
||||
// phi_z = std::atan2(-R47(1, 0), -R47(0, 0));
|
||||
// psi_z = 0.0;
|
||||
// }
|
||||
//
|
||||
// // 腕部分支调整
|
||||
// if (wrist_config_ == INWARD) {
|
||||
// phi_z += M_PI;
|
||||
// theta_y = -theta_y;
|
||||
// psi_z += M_PI;
|
||||
// }
|
||||
k = wrist_config_; // +1 / -1
|
||||
|
||||
// 处理奇异情况
|
||||
if (std::sin(theta_y) < 1e-12) {
|
||||
phi_z = std::atan2(R47(1, 0), R47(0, 0));
|
||||
// ===== 1. 通用 ZYZ 提取=====
|
||||
double c = std::clamp(R47(2, 2), -1.0, 1.0); // clip
|
||||
double theta_y = k * std::acos(c);
|
||||
// 另一种写法（注释里那行）：
|
||||
// double theta_y = std::atan2(k * std::hypot(R47(2,0), R47(2,1)), R47(2,2));
|
||||
|
||||
double phi_z = std::atan2(k * R47(1, 2), k * R47(0, 2));
|
||||
double psi_z = std::atan2(k * R47(2, 1), -k * R47(2, 0));
|
||||
|
||||
// ===== 2. 奇异退化处理 =====
|
||||
if (std::fabs(c - 1.0) < eps)
|
||||
{
|
||||
// theta ≈ 0
|
||||
theta_y = 0.0;
|
||||
phi_z = std::atan2(k * R47(1, 0), k * R47(0, 0));
|
||||
psi_z = 0.0;
|
||||
}
|
||||
if (std::sin(M_PI - theta_y) < 1e-12) {
|
||||
phi_z = std::atan2(-R47(1, 0), -R47(0, 0));
|
||||
else if (std::fabs(c + 1.0) < eps)
|
||||
{
|
||||
// theta ≈ ±π
|
||||
theta_y = k * M_PI;
|
||||
phi_z = std::atan2(-k * R47(1, 0), -k * R47(0, 0));
|
||||
psi_z = 0.0;
|
||||
}
|
||||
|
||||
// 腕部分支调整
|
||||
if (wrist_config_ == INWARD) {
|
||||
phi_z += M_PI;
|
||||
theta_y = -theta_y;
|
||||
psi_z += M_PI;
|
||||
}
|
||||
|
||||
// 调整角度
|
||||
joints[4] = SupportFunctions::normalize_angle(phi_z - M_PI / 2);
|
||||
joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2);
|
||||
@ -233,7 +298,8 @@ Eigen::Matrix4d BiasSRSIkSolver::calc_total_transform(const std::vector<double>
|
||||
return T_total;
|
||||
}
|
||||
|
||||
bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen::MatrixXd &s_mat, Eigen::MatrixXd &w_mat) {
|
||||
bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen::MatrixXd &s_mat,
|
||||
Eigen::MatrixXd &w_mat) {
|
||||
if (s_mat.rows() != 3 || s_mat.cols() != 9) s_mat.setZero(3, 9);
|
||||
if (w_mat.rows() != 3 || w_mat.cols() != 9) w_mat.setZero(3, 9);
|
||||
|
||||
@ -255,7 +321,17 @@ bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen:
|
||||
|
||||
|
||||
double d_sw = (W - S).norm();
|
||||
if ((std::abs(d_se_ + d_ew_) < d_sw) || (d_sw < std::abs(d_se_ - d_ew_))) {
|
||||
const double r_max = d_se_ + d_ew_; // 最远：完全伸直
|
||||
const double r_min = std::abs(d_se_ - d_ew_); // 最近：完全折叠
|
||||
|
||||
// 与上下界的偏差
|
||||
double diff_max = d_sw - r_max; // >0 表示比 r_max 还远
|
||||
double diff_min = r_min - d_sw; // >0 表示比 r_min 还近
|
||||
|
||||
// 明显超过可达范围才判定为不可达：
|
||||
// - diff_max > EPS → 太远
|
||||
// - diff_min > EPS → 太近
|
||||
if (diff_max > EPS || diff_min > EPS) {
|
||||
throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
|
||||
}
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
|
||||
|
||||
#include "ik_solver/include/ik_solver_creator.h"
|
||||
#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_limit_bias_solver.h"
|
||||
#include "ik_solver/include/opt_psi_limit_bias_solver.h"
|
||||
namespace cmvr
|
||||
{
|
||||
std::unordered_map<IkSolverType, std::shared_ptr<IKSolver>>
|
||||
663
src/ik_solver/src/ik_test.cpp
Normal file
663
src/ik_solver/src/ik_test.cpp
Normal file
@ -0,0 +1,663 @@
|
||||
//
|
||||
// Created by lgv on 11/28/25.
|
||||
//
|
||||
|
||||
|
||||
|
||||
#include <iostream>
|
||||
#include <pinocchio/parsers/urdf.hpp>
|
||||
#include <pinocchio/algorithm/model.hpp>
|
||||
#include <pinocchio/algorithm/kinematics.hpp>
|
||||
#include <pinocchio/algorithm/frames.hpp>
|
||||
#include <pinocchio/algorithm/jacobian.hpp>
|
||||
#include "pinocchio/multibody/sample-models.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include "gtest/gtest.h"
|
||||
#include "ik_solver/include/pinocchio_dls_ik_solver.h"
|
||||
#include "ik_solver/include/pinocchio_qp_ik_solver.h"
|
||||
#include "ik_solver/include/opt_psi_limit_bias_solver.h"
|
||||
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
|
||||
using namespace cmvr;
|
||||
|
||||
|
||||
class DualArmViewer : public MuJocoViewer {
|
||||
public:
|
||||
using Vec7 = std::vector<double>;
|
||||
|
||||
explicit DualArmViewer(const char *model_path)
|
||||
: MuJocoViewer(model_path),
|
||||
q_cmd_(7, 0.0) { // 初始 7 维全 0
|
||||
}
|
||||
|
||||
// 更新右臂 7 关节的目标角度（rad）
|
||||
// 顺序：
|
||||
// [R_SHOULDER_P, R_SHOULDER_R, R_SHOULDER_Y,
|
||||
// R_ELBOW_R, R_WRIST_P, R_WRIST_Y, R_WRIST_R]
|
||||
void moveJ(const Vec7 &q_target) {
|
||||
std::lock_guard<std::mutex> lk(mtx_);
|
||||
q_cmd_ = q_target; // 只更新目标，不直接改 d->ctrl
|
||||
}
|
||||
|
||||
protected:
|
||||
// 只在第一次进控制循环时调用
|
||||
void initOnce(mjModel *m, mjData *d) override {
|
||||
UNUSED_VARIABLE(d);
|
||||
|
||||
// actuator 名（已经有）
|
||||
const char *act_names[7] = {
|
||||
"R_SHOULDER_P_pos",
|
||||
"R_SHOULDER_R_pos",
|
||||
"R_SHOULDER_Y_pos",
|
||||
"R_ELBOW_R_pos",
|
||||
"R_WRIST_P_pos",
|
||||
"R_WRIST_Y_pos",
|
||||
"R_WRIST_R_pos"
|
||||
};
|
||||
|
||||
// 对应的 joint 名
|
||||
const char *jnt_names[7] = {
|
||||
"R_SHOULDER_P",
|
||||
"R_SHOULDER_R",
|
||||
"R_SHOULDER_Y",
|
||||
"R_ELBOW_R",
|
||||
"R_WRIST_P",
|
||||
"R_WRIST_Y",
|
||||
"R_WRIST_R"
|
||||
};
|
||||
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
// 1. 保存 actuator id
|
||||
int act_id = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
|
||||
right_act_ids_[i] = act_id;
|
||||
if (act_id < 0) {
|
||||
std::cerr << "[DualArmViewer] actuator not found: "
|
||||
<< act_names[i] << std::endl;
|
||||
}
|
||||
|
||||
// 2. 保存 joint id
|
||||
int jnt_id = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]);
|
||||
right_jnt_ids_[i] = jnt_id;
|
||||
if (jnt_id < 0) {
|
||||
std::cerr << "[DualArmViewer] joint not found: "
|
||||
<< jnt_names[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
act_ids_inited_ = true;
|
||||
}
|
||||
|
||||
// 每个 mj_step 前会被 physics 线程调用
|
||||
void controlCallback(mjModel *m, mjData *d) override {
|
||||
if (!act_ids_inited_) return;
|
||||
|
||||
Vec7 q_local(7, 0.0);
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx_);
|
||||
q_local = q_cmd_; // 拷贝一份当前目标，避免长时间持锁
|
||||
}
|
||||
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
int act_id = right_act_ids_[i];
|
||||
if (act_id < 0) continue;
|
||||
d->ctrl[act_id] = q_local[i];
|
||||
}
|
||||
|
||||
// // 示例：读取第 5 个关节（R_WRIST_P）的实际角度并打印
|
||||
// int jnt_id = right_jnt_ids_[4];
|
||||
// if (jnt_id >= 0) {
|
||||
// int qpos_adr = m->jnt_qposadr[jnt_id];
|
||||
// double q_actual = d->qpos[qpos_adr];
|
||||
// std::cout << "cmd: " << q_local[4]
|
||||
// << " act: " << q_actual << std::endl;
|
||||
// }
|
||||
}
|
||||
|
||||
void onReset(mjModel *m, mjData *d) override {
|
||||
UNUSED_VARIABLE(m);
|
||||
UNUSED_VARIABLE(d);
|
||||
std::lock_guard<std::mutex> lk(mtx_);
|
||||
q_cmd_.assign(7, 0.0); // reset 时把目标清零，保持 7 维
|
||||
}
|
||||
|
||||
private:
|
||||
std::array<int, 7> right_act_ids_{}; // 右臂 7 个 actuator id
|
||||
std::array<int, 7> right_jnt_ids_{}; // 右臂 7 个 joint id（新加）
|
||||
bool act_ids_inited_{false};
|
||||
|
||||
Vec7 q_cmd_; // 当前命令目标角（始终 7 维）
|
||||
mutable std::mutex mtx_; // 保护 q_cmd_
|
||||
};
|
||||
|
||||
|
||||
// ---------- 工具函数：计算均值 + 标准差 ----------
|
||||
static void computeMeanStd(const std::vector<double> &data,
|
||||
double &mean, double &stddev)
|
||||
{
|
||||
if (data.empty()) {
|
||||
mean = stddev = std::numeric_limits<double>::quiet_NaN();
|
||||
return;
|
||||
}
|
||||
double sum = 0.0;
|
||||
for (double x : data) sum += x;
|
||||
mean = sum / static_cast<double>(data.size());
|
||||
|
||||
double var = 0.0;
|
||||
if (data.size() > 1) {
|
||||
for (double x : data) {
|
||||
double d = x - mean;
|
||||
var += d * d;
|
||||
}
|
||||
var /= static_cast<double>(data.size() - 1);
|
||||
}
|
||||
stddev = std::sqrt(var);
|
||||
}
|
||||
|
||||
// 姿态误差：两旋转矩阵之间的角度（弧度制）
|
||||
static double rotationError(const Eigen::Matrix3d &R_des,
|
||||
const Eigen::Matrix3d &R_cur)
|
||||
{
|
||||
Eigen::Matrix3d R_err = R_des.transpose() * R_cur;
|
||||
double cos_theta = (R_err.trace() - 1.0) * 0.5;
|
||||
if (cos_theta > 1.0) cos_theta = 1.0;
|
||||
if (cos_theta < -1.0) cos_theta = -1.0;
|
||||
return std::acos(cos_theta); // rad
|
||||
}
|
||||
|
||||
struct SolverStats {
|
||||
std::string name;
|
||||
std::vector<double> times_ms; // 单次成功求解耗时
|
||||
std::vector<double> pos_errors; // 末端位置误差 (m)
|
||||
std::vector<double> ori_errors; // 姿态误差 (rad)
|
||||
int attempts = 0; // 尝试次数（有 target_pose 就算一次）
|
||||
int success = 0; // 成功次数（误差在阈值内）
|
||||
};
|
||||
|
||||
void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
|
||||
{
|
||||
// ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ==========
|
||||
const std::array<std::pair<double,double>, 7> joints_limits_ = {{
|
||||
{-0.26, 1.57},
|
||||
{-0.78, 1.57},
|
||||
{-3.1415, 3.1415},
|
||||
{ 0.0, 2.05},
|
||||
{-3.1415, 3.1415},
|
||||
{-0.78, 0.78},
|
||||
{-0.26, 1.57},
|
||||
}};
|
||||
|
||||
constexpr int N_SAMPLES = 100; // 样本数：1000 组随机关节角
|
||||
|
||||
// ========== 2. 创建三个求解器实例 ==========
|
||||
|
||||
// 数值优化类 QP IK
|
||||
PinocchioQpIKSolver qp_solver(
|
||||
"/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
|
||||
"PELVIS_S",
|
||||
"R_WRIST_R_S", // 法兰 frame
|
||||
"R_FINGER_TIP_FIXED" // TCP frame
|
||||
);
|
||||
|
||||
// 基于广义逆雅可比矩阵的数值增量 IK
|
||||
PinocchioDlsIKSolver pinv_solver(
|
||||
"/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
|
||||
"PELVIS_S",
|
||||
"R_WRIST_R_S", // 法兰 frame
|
||||
"R_FINGER_TIP_FIXED" // TCP frame
|
||||
);
|
||||
|
||||
// 你的专利方法：臂角 ψ + 可行域 + 势场
|
||||
OptPsiLimitBiasSolver psi_solver;
|
||||
|
||||
// 统一容器，方便 for 循环
|
||||
std::vector<IKSolver*> solvers = {
|
||||
&psi_solver,
|
||||
// &pinv_solver,
|
||||
// &qp_solver
|
||||
};
|
||||
|
||||
|
||||
std::vector<SolverStats> stats = {
|
||||
{"OptPsiLimitBias (Patent)", {}, {}, {}, 0, 0},
|
||||
{"Pinocchio Jacobian IK", {}, {}, {}, 0, 0},
|
||||
{"Pinocchio QP IK", {}, {}, {}, 0, 0}
|
||||
};
|
||||
|
||||
// ========== 3. 初始化（如果类里有 init 就调一下） ==========
|
||||
for (auto *solver : solvers) {
|
||||
if (!solver->init()) {
|
||||
std::cerr << "Warning: solver init() failed.\n";
|
||||
}
|
||||
}
|
||||
|
||||
// ========== 4. 随机数发生器 ==========
|
||||
std::mt19937 rng(42); // 固定种子，结果可复现
|
||||
std::uniform_real_distribution<double> dist01(0.0, 1.0);
|
||||
|
||||
// ========== 5. 主循环：随机关节角 → FK → 三种 IK ==========
|
||||
// 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值”
|
||||
for (int i = 0; i < N_SAMPLES; ++i) {
|
||||
// 5.1 随机生成一组 q_true
|
||||
std::vector<double> q_true(7);
|
||||
for (int j = 0; j < 7; ++j) {
|
||||
double r = dist01(rng); // [0, 1]
|
||||
double qmin = joints_limits_[j].first;
|
||||
double qmax = joints_limits_[j].second;
|
||||
q_true[j] = qmin + r * (qmax - qmin);
|
||||
}
|
||||
|
||||
// 5.3 为这一组样本生成一个“统一的初始解” q_init（和 q_true 无关）
|
||||
std::vector<double> q_init(7);
|
||||
for (int j = 0; j < 7; ++j) {
|
||||
double r = dist01(rng);
|
||||
double qmin = joints_limits_[j].first;
|
||||
double qmax = joints_limits_[j].second;
|
||||
q_init[j] = qmin + r * (qmax - qmin);
|
||||
}
|
||||
|
||||
// std::vector<double> q_cur(7, 0.0);
|
||||
// // q_true = q_cur;
|
||||
// Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
|
||||
// if (!psi_solver.fk(q_true, target_pose, true)) {
|
||||
// // FK 失败的话，这个样本就跳过
|
||||
// continue;
|
||||
// }
|
||||
//
|
||||
// Eigen::Matrix4d target_pose1 = Eigen::Matrix4d::Identity();
|
||||
// if (!pinv_solver.fk(q_true, target_pose1, true)) {
|
||||
// // FK 失败的话，这个样本就跳过
|
||||
// continue;
|
||||
// }
|
||||
//
|
||||
// Eigen::Matrix4d target_pose2 = Eigen::Matrix4d::Identity();
|
||||
// if (!qp_solver.fk(q_true, target_pose2, true)) {
|
||||
// // FK 失败的话，这个样本就跳过
|
||||
// continue;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// 5.3 三个 IK 分别求解
|
||||
for (size_t idx = 0; idx < solvers.size(); ++idx) {
|
||||
|
||||
IKSolver *solver = solvers[idx];
|
||||
SolverStats &s = stats[idx];
|
||||
s.attempts++;
|
||||
|
||||
// 5.2 用 pinv_solver 的 FK 计算目标末端位姿 target_pose
|
||||
|
||||
Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
|
||||
if (!solver->fk(q_true, target_pose, false)) {
|
||||
// FK 失败的话，这个样本就跳过
|
||||
continue;
|
||||
}
|
||||
|
||||
// 先用真实关节角更新内部状态（按你之前的约定）
|
||||
// std::vector<double> q_cur(7, 0.0);
|
||||
solver->update_joints_state(q_init);
|
||||
|
||||
std::vector<double> q_sol;
|
||||
q_sol.reserve(7);
|
||||
|
||||
auto t0 = std::chrono::steady_clock::now();
|
||||
bool ok = solver->ik(target_pose, q_sol, false); // is_tcp = true
|
||||
auto t1 = std::chrono::steady_clock::now();
|
||||
|
||||
if (!ok || q_sol.size() != 7) {
|
||||
// 求解失败，不计入成功统计
|
||||
continue;
|
||||
}
|
||||
|
||||
viewer.moveJ(q_sol);
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(1));
|
||||
|
||||
double dt_ms = std::chrono::duration_cast<
|
||||
std::chrono::microseconds>(t1 - t0).count() / 1000.0;
|
||||
|
||||
// 5.4 用该求解器自己的 FK 得到实际末端位姿 pose_sol
|
||||
Eigen::Matrix4d pose_sol = Eigen::Matrix4d::Identity();
|
||||
if (!solver->fk(q_sol, pose_sol, false)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 5.5 计算末端位置误差 + 姿态误差
|
||||
Eigen::Vector3d p_des = target_pose.block<3,1>(0,3);
|
||||
Eigen::Vector3d p_cur = pose_sol.block<3,1>(0,3);
|
||||
double pos_err = (p_cur - p_des).norm(); // m
|
||||
|
||||
Eigen::Matrix3d R_des = target_pose.block<3,3>(0,0);
|
||||
Eigen::Matrix3d R_cur = pose_sol.block<3,3>(0,0);
|
||||
double ori_err = rotationError(R_des, R_cur); // rad
|
||||
|
||||
// 5.6 判定是否“成功解”
|
||||
// 阈值可以按你机械臂精度需求调
|
||||
const double POS_THRESH = 1e-3; // 0.1 mm
|
||||
const double ORI_THRESH = 1e-3; // ≈ 0.057°
|
||||
if (pos_err < POS_THRESH && ori_err < ORI_THRESH) {
|
||||
s.success++;
|
||||
s.times_ms.push_back(dt_ms);
|
||||
s.pos_errors.push_back(pos_err);
|
||||
s.ori_errors.push_back(ori_err);
|
||||
} else {
|
||||
// 这里是“收敛但精度不够”的情况，如果你想也可以单独统计
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// ========== 6. 输出统计结果：耗时/误差（均值 + 标准差）+ 成功率 ==========
|
||||
for (const auto &s : stats) {
|
||||
std::cout << "========== Solver: " << s.name << " ==========\n";
|
||||
std::cout << "Attempts: " << s.attempts
|
||||
<< ", Success: " << s.success;
|
||||
if (s.attempts > 0) {
|
||||
double succ_rate =
|
||||
100.0 * static_cast<double>(s.success) /
|
||||
static_cast<double>(s.attempts);
|
||||
std::cout << " (Success rate: " << succ_rate << "%)\n";
|
||||
} else {
|
||||
std::cout << " (No attempts)\n";
|
||||
}
|
||||
|
||||
double mean_t, std_t;
|
||||
computeMeanStd(s.times_ms, mean_t, std_t);
|
||||
|
||||
double mean_ep, std_ep;
|
||||
computeMeanStd(s.pos_errors, mean_ep, std_ep);
|
||||
|
||||
double mean_er, std_er;
|
||||
computeMeanStd(s.ori_errors, mean_er, std_er);
|
||||
|
||||
std::cout << "Time (ms) : mean = " << mean_t
|
||||
<< ", std = " << std_t << "\n";
|
||||
std::cout << "PosErr (m) : mean = " << mean_ep
|
||||
<< ", std = " << std_ep << "\n";
|
||||
std::cout << "OriErr (rad) : mean = " << mean_er
|
||||
<< ", std = " << std_er << "\n\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void benchmarkIkSolversRandomJoints()
|
||||
{
|
||||
// ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ==========
|
||||
const std::array<std::pair<double,double>, 7> joints_limits_ = {{
|
||||
|
||||
{-0.26, 1.57},
|
||||
{-0.78, 1.57},
|
||||
{-M_PI, M_PI},
|
||||
{ 0.0, 2.05},
|
||||
{-M_PI, M_PI},
|
||||
{-0.78, 0.78},
|
||||
{-0.26, 1.57},
|
||||
|
||||
}};
|
||||
|
||||
constexpr int N_SAMPLES = 10000; // 样本数：1000 组随机关节角
|
||||
|
||||
// ========== 2. 创建三个求解器实例 ==========
|
||||
|
||||
// 数值优化类 QP IK
|
||||
PinocchioQpIKSolver qp_solver(
|
||||
"/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
|
||||
"PELVIS_S",
|
||||
"R_WRIST_R_S", // 法兰 frame
|
||||
"R_FINGER_TIP_FIXED" // TCP frame
|
||||
);
|
||||
|
||||
// 基于广义逆雅可比矩阵的数值增量 IK
|
||||
PinocchioDlsIKSolver pinv_solver(
|
||||
"/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
|
||||
"PELVIS_S",
|
||||
"R_WRIST_R_S", // 法兰 frame
|
||||
"R_FINGER_TIP_FIXED" // TCP frame
|
||||
);
|
||||
|
||||
// 你的专利方法：臂角 ψ + 可行域 + 势场
|
||||
OptPsiLimitBiasSolver psi_solver;
|
||||
|
||||
// 统一容器，方便 for 循环
|
||||
std::vector<IKSolver*> solvers = {
|
||||
&psi_solver,
|
||||
// &pinv_solver,
|
||||
// &qp_solver
|
||||
};
|
||||
|
||||
|
||||
std::vector<SolverStats> stats = {
|
||||
{"OptPsiLimitBias (Patent)", {}, {}, {}, 0, 0},
|
||||
{"Pinocchio Jacobian IK", {}, {}, {}, 0, 0},
|
||||
{"Pinocchio QP IK", {}, {}, {}, 0, 0}
|
||||
};
|
||||
|
||||
// ========== 3. 初始化（如果类里有 init 就调一下） ==========
|
||||
for (auto *solver : solvers) {
|
||||
if (!solver->init()) {
|
||||
std::cerr << "Warning: solver init() failed.\n";
|
||||
}
|
||||
}
|
||||
|
||||
// ========== 4. 随机数发生器 ==========
|
||||
std::mt19937 rng(42); // 固定种子，结果可复现
|
||||
std::uniform_real_distribution<double> dist01(0.0, 1.0);
|
||||
|
||||
// ========== 5. 主循环：随机关节角 → FK → 三种 IK ==========
|
||||
// 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值”
|
||||
for (int i = 0; i < N_SAMPLES; ++i) {
|
||||
// 5.1 随机生成一组 q_true
|
||||
std::vector<double> q_true(7);
|
||||
for (int j = 0; j < 7; ++j) {
|
||||
double r = dist01(rng); // [0, 1]
|
||||
double qmin = joints_limits_[j].first;
|
||||
double qmax = joints_limits_[j].second;
|
||||
q_true[j] = qmin + r * (qmax - qmin);
|
||||
}
|
||||
|
||||
// 5.3 为这一组样本生成一个“统一的初始解” q_init（和 q_true 无关）
|
||||
std::vector<double> q_init(7);
|
||||
for (int j = 0; j < 7; ++j) {
|
||||
double r = dist01(rng);
|
||||
double qmin = joints_limits_[j].first;
|
||||
double qmax = joints_limits_[j].second;
|
||||
q_init[j] = qmin + r * (qmax - qmin);
|
||||
}
|
||||
|
||||
// std::vector<double> q_cur(7, 0.0);
|
||||
// // q_true = q_cur;
|
||||
// Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
|
||||
// if (!psi_solver.fk(q_true, target_pose, true)) {
|
||||
// // FK 失败的话，这个样本就跳过
|
||||
// continue;
|
||||
// }
|
||||
//
|
||||
// Eigen::Matrix4d target_pose1 = Eigen::Matrix4d::Identity();
|
||||
// if (!pinv_solver.fk(q_true, target_pose1, true)) {
|
||||
// // FK 失败的话，这个样本就跳过
|
||||
// continue;
|
||||
// }
|
||||
//
|
||||
// Eigen::Matrix4d target_pose2 = Eigen::Matrix4d::Identity();
|
||||
// if (!qp_solver.fk(q_true, target_pose2, true)) {
|
||||
// // FK 失败的话，这个样本就跳过
|
||||
// continue;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// 5.3 三个 IK 分别求解
|
||||
for (size_t idx = 0; idx < solvers.size(); ++idx) {
|
||||
|
||||
IKSolver *solver = solvers[idx];
|
||||
SolverStats &s = stats[idx];
|
||||
s.attempts++;
|
||||
|
||||
// 5.2 用 pinv_solver 的 FK 计算目标末端位姿 target_pose
|
||||
|
||||
Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
|
||||
if (!solver->fk(q_true, target_pose, false)) {
|
||||
// FK 失败的话，这个样本就跳过
|
||||
continue;
|
||||
}
|
||||
|
||||
// 先用真实关节角更新内部状态（按你之前的约定）
|
||||
// std::vector<double> q_cur(7, 0.0);
|
||||
solver->update_joints_state(q_init);
|
||||
|
||||
std::vector<double> q_sol;
|
||||
q_sol.reserve(7);
|
||||
|
||||
auto t0 = std::chrono::steady_clock::now();
|
||||
bool ok = solver->ik(target_pose, q_sol, false); // is_tcp = true
|
||||
auto t1 = std::chrono::steady_clock::now();
|
||||
|
||||
if (!ok || q_sol.size() != 7) {
|
||||
// 求解失败，不计入成功统计
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
double dt_ms = std::chrono::duration_cast<
|
||||
std::chrono::microseconds>(t1 - t0).count() / 1000.0;
|
||||
|
||||
// 5.4 用该求解器自己的 FK 得到实际末端位姿 pose_sol
|
||||
Eigen::Matrix4d pose_sol = Eigen::Matrix4d::Identity();
|
||||
if (!solver->fk(q_sol, pose_sol, false)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 5.5 计算末端位置误差 + 姿态误差
|
||||
Eigen::Vector3d p_des = target_pose.block<3,1>(0,3);
|
||||
Eigen::Vector3d p_cur = pose_sol.block<3,1>(0,3);
|
||||
double pos_err = (p_cur - p_des).norm(); // m
|
||||
|
||||
Eigen::Matrix3d R_des = target_pose.block<3,3>(0,0);
|
||||
Eigen::Matrix3d R_cur = pose_sol.block<3,3>(0,0);
|
||||
double ori_err = rotationError(R_des, R_cur); // rad
|
||||
|
||||
// 5.6 判定是否“成功解”
|
||||
// 阈值可以按你机械臂精度需求调
|
||||
const double POS_THRESH = 1e-3; // 0.1 mm
|
||||
const double ORI_THRESH = 1e-3; // ≈ 0.057°
|
||||
if (pos_err < POS_THRESH && ori_err < ORI_THRESH) {
|
||||
s.success++;
|
||||
s.times_ms.push_back(dt_ms);
|
||||
s.pos_errors.push_back(pos_err);
|
||||
s.ori_errors.push_back(ori_err);
|
||||
} else {
|
||||
// 这里是“收敛但精度不够”的情况，如果你想也可以单独统计
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// ========== 6. 输出统计结果：耗时/误差（均值 + 标准差）+ 成功率 ==========
|
||||
for (const auto &s : stats) {
|
||||
std::cout << "========== Solver: " << s.name << " ==========\n";
|
||||
std::cout << "Attempts: " << s.attempts
|
||||
<< ", Success: " << s.success;
|
||||
if (s.attempts > 0) {
|
||||
double succ_rate =
|
||||
100.0 * static_cast<double>(s.success) /
|
||||
static_cast<double>(s.attempts);
|
||||
std::cout << " (Success rate: " << succ_rate << "%)\n";
|
||||
} else {
|
||||
std::cout << " (No attempts)\n";
|
||||
}
|
||||
|
||||
double mean_t, std_t;
|
||||
computeMeanStd(s.times_ms, mean_t, std_t);
|
||||
|
||||
double mean_ep, std_ep;
|
||||
computeMeanStd(s.pos_errors, mean_ep, std_ep);
|
||||
|
||||
double mean_er, std_er;
|
||||
computeMeanStd(s.ori_errors, mean_er, std_er);
|
||||
|
||||
std::cout << "Time (ms) : mean = " << mean_t
|
||||
<< ", std = " << std_t << "\n";
|
||||
std::cout << "PosErr (m) : mean = " << mean_ep
|
||||
<< ", std = " << std_ep << "\n";
|
||||
std::cout << "OriErr (rad) : mean = " << mean_er
|
||||
<< ", std = " << std_er << "\n\n";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ik_test,pinocchio_lib_test) {
|
||||
// PinocchioQpIKSolver solver(
|
||||
// "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
|
||||
// "PELVIS_S",
|
||||
// "R_WRIST_R_S", // 法兰 frame
|
||||
// "R_FINGER_TIP_FIXED" // TCP frame
|
||||
// );
|
||||
|
||||
OptPsiLimitBiasSolver solver;
|
||||
solver.init();
|
||||
|
||||
|
||||
// 真实关节角（比如从控制器读回来）
|
||||
std::vector<double> q_cur(7, 0.0);
|
||||
solver.update_joints_state(q_cur);
|
||||
|
||||
q_cur[5] = 0.236;
|
||||
q_cur[4] = -0.236;
|
||||
q_cur[3] = 0.156;
|
||||
q_cur[2] = 0.036;
|
||||
q_cur[1] = 0.236;
|
||||
|
||||
// 1) 先求当前 TCP 位姿
|
||||
Eigen::Matrix4d cur_tcp_pose;
|
||||
solver.fk(q_cur, cur_tcp_pose, false); // is_tcp = true
|
||||
std::cout << cur_tcp_pose<< std::endl;
|
||||
|
||||
|
||||
Eigen::Matrix4d target_tcp_pose = cur_tcp_pose;
|
||||
|
||||
|
||||
std::vector<double> q_target;
|
||||
bool ok = solver.ik(target_tcp_pose, q_target, false); // is_tcp = true
|
||||
|
||||
for (double q: q_target) {
|
||||
std::cout << q << std::endl;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d cur_flange_pose;
|
||||
solver.fk(q_target, cur_flange_pose, false); // is_tcp = false
|
||||
std::cout << cur_flange_pose<< std::endl;
|
||||
}
|
||||
|
||||
TEST(ik_test,ik_compare) {
|
||||
benchmarkIkSolversRandomJoints();
|
||||
|
||||
|
||||
//
|
||||
// const char *model_path =
|
||||
// "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml";
|
||||
//
|
||||
// DualArmViewer viewer(model_path);
|
||||
// std::this_thread::sleep_for(std::chrono::seconds(3));
|
||||
//
|
||||
// // int viewer = 0;
|
||||
// // 把所有 IK 运算 + moveJ 循环放到控制线程里
|
||||
// std::thread ctrl_thread([&viewer]() {
|
||||
// benchmarkIkSolversRandomJoints(viewer);
|
||||
//
|
||||
// //
|
||||
// // // 可视化：右臂关节位置控制
|
||||
// // viewer.moveJ(q); // 更新目标角
|
||||
// // std::this_thread::sleep_for(std::chrono::duration<double>(0.01));
|
||||
//
|
||||
//
|
||||
// });
|
||||
//
|
||||
//
|
||||
// viewer.run(); // 阻塞，直到你关掉窗口
|
||||
// ctrl_thread.join(); // 控制线程结束
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -2,46 +2,132 @@
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
#include "opt_psi_limit_bias_solver/include/joints_limit_analyzer.h"
|
||||
#include "ik_solver/include/joints_limit_analyzer.h"
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
using namespace cmvr;
|
||||
|
||||
|
||||
//
|
||||
// bool JointsLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double bd, double cn, double cd,
|
||||
// double theta_target, double psi) {
|
||||
// // 由 ψ 求 θ：θ = atan2( N, D )
|
||||
// // N = an*sinψ + bn*cosψ + cn
|
||||
// // D = ad*sinψ + bd*cosψ + cd
|
||||
// double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
// double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
// double theta = std::atan2(N, D);
|
||||
// double err = SupportFunctions::normalize_angle(theta - theta_target);
|
||||
// return std::abs(err) < 1e-6;
|
||||
// }
|
||||
|
||||
bool JointsLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double bd, double cn, double cd,
|
||||
double theta_target, double psi) {
|
||||
// 由 ψ 求 θ：θ = atan2( N, D )
|
||||
// N = an*sinψ + bn*cosψ + cn
|
||||
// D = ad*sinψ + bd*cosψ + cd
|
||||
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
double theta = std::atan2(N, D);
|
||||
double err = SupportFunctions::normalize_angle(theta - theta_target);
|
||||
return std::abs(err) < 1e-6;
|
||||
bool JointsLimitAnalyzer::check_tan_solution(
|
||||
double an, double ad, double bn, double bd, double cn, double cd,
|
||||
double theta_target, double psi)
|
||||
{
|
||||
// 目标角的正弦、余弦
|
||||
const double s = std::sin(theta_target);
|
||||
const double c = std::cos(theta_target);
|
||||
|
||||
// 当前 psi 对应的 N、D
|
||||
const double sp = std::sin(psi);
|
||||
const double cp = std::cos(psi);
|
||||
const double N = an * sp + bn * cp + cn;
|
||||
const double D = ad * sp + bd * cp + cd;
|
||||
|
||||
// 1) cross ~= 0: 保证 (N,D) 和 (s,c) 共线
|
||||
const double cross = D * s - N * c;
|
||||
const double scale = std::max(std::abs(N) + std::abs(D), 1.0);
|
||||
if (std::abs(cross) > 1e-8 * scale) { // 容差可以按你 EPS 调
|
||||
return false;
|
||||
}
|
||||
|
||||
// 2) dot > 0: 保证方向一致，而不是差了 π
|
||||
const double dot = N * s + D * c;
|
||||
if (dot <= 0.0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// std::vector<double> JointsLimitAnalyzer::calc_tan_solution(
|
||||
// double an, double ad, double bn, double bd, double cn, double cd, double theta) {
|
||||
// std::vector<double> out;
|
||||
//
|
||||
// // 用 sin/cos(θ) 形成方程： (D sinθ - N cosθ) = 0
|
||||
// const double s = std::sin(theta);
|
||||
// const double c = std::cos(theta);
|
||||
//
|
||||
// // 二次式 A t^2 + B t + C = 0, t = tan(ψ/2)
|
||||
// double A = s * (cd - bd) - c * (cn - bn);
|
||||
// double B = 2.0 * (s * ad - c * an);
|
||||
// double C = s * (bd + cd) - c * (bn + cn);
|
||||
//
|
||||
// // 退化线性兜底
|
||||
// if (std::abs(A) < EPS) {
|
||||
// if (std::abs(B) < EPS) {
|
||||
// // A≈0 且 B≈0：按无解处理。
|
||||
// if (std::abs(C) < EPS) {
|
||||
// // 恒等：任意 ψ 都满足——按需要返回空或[-π,π]
|
||||
// return {-M_PI,M_PI};
|
||||
// }
|
||||
// return {};
|
||||
// }
|
||||
// // 线性：B t + C = 0
|
||||
// double psi = 2.0 * std::atan2(-C, B);
|
||||
// psi = SupportFunctions::normalize_angle(psi);
|
||||
// if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi))
|
||||
// return {psi};
|
||||
// return {};
|
||||
// }
|
||||
//
|
||||
// // 判别式（带稳健夹零）
|
||||
// double D = B * B - 4.0 * A * C;
|
||||
// if (D < -1e-14 * (A * A + B * B + C * C)) return {}; // 为负，无解
|
||||
// D = std::max(0.0, D);
|
||||
// double sqrtD = std::sqrt(D);
|
||||
//
|
||||
// // t = (-B ± sqrtD)/(2A) → ψ = 2*atan(t)
|
||||
// double psi1 = 2.0 * std::atan2(-(B - sqrtD), 2.0 * A);
|
||||
// double psi2 = 2.0 * std::atan2(-(B + sqrtD), 2.0 * A);
|
||||
//
|
||||
// psi1 = SupportFunctions::normalize_angle(psi1);
|
||||
// psi2 = SupportFunctions::normalize_angle(psi2);
|
||||
//
|
||||
// if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi1))
|
||||
// out.push_back(psi1);
|
||||
// if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi2) &&
|
||||
// std::abs(psi2 - psi1) > EPS)
|
||||
// out.push_back(psi2);
|
||||
//
|
||||
// std::sort(out.begin(), out.end());
|
||||
// return out;
|
||||
// }
|
||||
|
||||
|
||||
std::vector<double> JointsLimitAnalyzer::calc_tan_solution(
|
||||
double an, double ad, double bn, double bd, double cn, double cd, double theta) {
|
||||
std::vector<double> out;
|
||||
|
||||
// 用 sin/cos(θ) 形成方程： (D sinθ - N cosθ) = 0
|
||||
std::vector<double> out;
|
||||
const double s = std::sin(theta);
|
||||
const double c = std::cos(theta);
|
||||
|
||||
// 二次式 A t^2 + B t + C = 0, t = tan(ψ/2)
|
||||
double A = s * (cd - bd) - c * (cn - bn);
|
||||
double B = 2.0 * (s * ad - c * an);
|
||||
double C = s * (bd + cd) - c * (bn + cn);
|
||||
|
||||
|
||||
// 退化线性兜底
|
||||
if (std::abs(A) < EPS) {
|
||||
if (std::abs(B) < EPS) {
|
||||
// A≈0 且 B≈0：按无解处理。
|
||||
if (std::abs(C) < 1e-12) {
|
||||
// 恒等：任意 ψ 都满足——按需要返回空或[-π,π]，这里返回空让上层判定。
|
||||
if (std::abs(C) < EPS) {
|
||||
// 恒等：任意 ψ 都满足——按需要返回空或[-π,π]
|
||||
return {-M_PI,M_PI};
|
||||
}
|
||||
return {};
|
||||
}
|
||||
@ -53,13 +139,20 @@ std::vector<double> JointsLimitAnalyzer::calc_tan_solution(
|
||||
return {};
|
||||
}
|
||||
|
||||
// 判别式（带稳健夹零）
|
||||
double D = B * B - 4.0 * A * C;
|
||||
if (D < -1e-14 * (A * A + B * B + C * C)) return {}; // 为负，无解
|
||||
D = std::max(0.0, D);
|
||||
double sqrtD = std::sqrt(D);
|
||||
double Disc = B * B - 4.0 * A * C;
|
||||
double scale2 = A * A + B * B + C * C;
|
||||
|
||||
if (Disc < 0.0 && scale2 > 0.0) {
|
||||
if (Disc > -1e-12 * scale2) { // 比原来的 1e-14 略宽一点
|
||||
Disc = 0.0;
|
||||
} else {
|
||||
return {}; // 确实无解
|
||||
}
|
||||
}
|
||||
|
||||
Disc = std::max(0.0, Disc);
|
||||
double sqrtD = std::sqrt(Disc);
|
||||
|
||||
// t = (-B ± sqrtD)/(2A) → ψ = 2*atan(t)
|
||||
double psi1 = 2.0 * std::atan2(-(B - sqrtD), 2.0 * A);
|
||||
double psi2 = 2.0 * std::atan2(-(B + sqrtD), 2.0 * A);
|
||||
|
||||
@ -77,6 +170,7 @@ std::vector<double> JointsLimitAnalyzer::calc_tan_solution(
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
|
||||
double cn, double cd, double joint_l,
|
||||
double joint_u) {
|
||||
@ -142,7 +236,8 @@ std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_tan_limits(dou
|
||||
double R = bounds[i + 1];
|
||||
if (R <= L) continue;
|
||||
|
||||
double probe = (L + R) * 0.5; // 取中点
|
||||
// double probe = (L + R) * 0.555; // 取中点
|
||||
double probe = L + 0.5 * (R - L);
|
||||
|
||||
if ( SupportFunctions::angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
|
||||
allow_pairs.emplace_back(L, R);
|
||||
@ -317,26 +412,99 @@ std::vector<double> JointsLimitAnalyzer::calc_cos_solution(double a, double b, d
|
||||
|
||||
|
||||
|
||||
// std::vector<std::pair<double, double> >
|
||||
// JointsLimitAnalyzer::cal_offset_limits(double joint_l, double joint_u, double offset) {
|
||||
// double pL = SupportFunctions::normalize_angle(joint_l + offset); // [-π, π]
|
||||
// double pU = SupportFunctions::normalize_angle(joint_u + offset); // [-π, π]
|
||||
//
|
||||
// std::vector<std::pair<double, double> > joint_ranges;
|
||||
//
|
||||
// if (! SupportFunctions::wraps(pL, pU)) {
|
||||
// // 单段
|
||||
// if (pU > pL + EPS) {
|
||||
// joint_ranges.push_back({pL, pU});
|
||||
// }
|
||||
// } else {
|
||||
// // 跨 ±π，拆成两段 [-π, pU] ∪ [pL, π]
|
||||
// if (pU > -M_PI + EPS) joint_ranges.push_back({-M_PI, pU});
|
||||
// if (M_PI > pL + EPS) joint_ranges.push_back({pL, M_PI});
|
||||
// }
|
||||
// return joint_ranges;
|
||||
// }
|
||||
|
||||
std::vector<std::pair<double, double>>
|
||||
JointsLimitAnalyzer::cal_offset_limits(double joint_l, double joint_u, double offset) {
|
||||
double pL = SupportFunctions::normalize_angle(joint_l + offset); // [-π, π]
|
||||
double pU = SupportFunctions::normalize_angle(joint_u + offset); // [-π, π]
|
||||
JointsLimitAnalyzer::cal_offset_limits(double joint_l,
|
||||
double joint_u,
|
||||
double offset)
|
||||
{
|
||||
std::vector<std::pair<double, double>> joint_ranges{};
|
||||
const double TWO_PI = 2.0 * M_PI;
|
||||
|
||||
std::vector<std::pair<double, double> > joint_ranges;
|
||||
|
||||
if (! SupportFunctions::wraps(pL, pU)) {
|
||||
// 单段
|
||||
if (pU > pL + EPS) {
|
||||
joint_ranges.push_back({pL, pU});
|
||||
}
|
||||
} else {
|
||||
// 跨 ±π，拆成两段 [-π, pU] ∪ [pL, π]
|
||||
if (pU > -M_PI + EPS) joint_ranges.push_back({-M_PI, pU});
|
||||
if (M_PI > pL + EPS) joint_ranges.push_back({pL, M_PI});
|
||||
}
|
||||
// 1) 非法区间：上限 <= 下限，当成空集
|
||||
if (joint_u <= joint_l + EPS) {
|
||||
return joint_ranges;
|
||||
}
|
||||
|
||||
// 原始区间长度
|
||||
const double span = joint_u - joint_l;
|
||||
|
||||
// 2) 覆盖整整一圈（或略大）：认为没有限位 → 全区间 [-π, π]
|
||||
// 这样 joint_l = -π, joint_u = π, 无论 offset 是多少，
|
||||
// 都会直接返回 [-π, π]，不会再出现空结果。
|
||||
if (span >= TWO_PI - EPS) {
|
||||
joint_ranges.emplace_back(-M_PI, M_PI);
|
||||
return joint_ranges;
|
||||
}
|
||||
|
||||
// 3) 正常情况：长度在 (0, 2π) 之间
|
||||
//
|
||||
// 定义 θ ∈ [joint_l, joint_u]
|
||||
// 加偏移：φ = θ + offset ∈ [joint_l + offset, joint_u + offset]，长度仍为 span
|
||||
// 再归一化：ψ = normalize_angle(φ) ∈ [-π, π]
|
||||
//
|
||||
// ψ 的值集在圆上是一段连续弧，投影到 [-π, π] 上，要么是一段，要么是两段。
|
||||
// 做法：
|
||||
// a = joint_l + offset
|
||||
// pL = normalize_angle(a) // 左端点映射到 [-π, π]
|
||||
// pU_unwrapped = pL + span // 不归一化的右端点（同一“展开”的圆上）
|
||||
//
|
||||
// 若 pU_unwrapped ≤ π：说明没穿过 +π，一段；
|
||||
// 若 pU_unwrapped > π：说明穿过 +π 一次，会拆成两段。
|
||||
|
||||
const double a = joint_l + offset;
|
||||
const double pL = SupportFunctions::normalize_angle(a); // ∈ [-π, π]
|
||||
const double pU_unwrapped = pL + span; // 不归一化的右端点
|
||||
|
||||
auto push_if_valid = [&](double L, double U) {
|
||||
// 丢掉长度太小的段
|
||||
if (U <= L + EPS) return;
|
||||
|
||||
// 保险起见，再裁剪到 [-π, π]
|
||||
if (L < -M_PI) L = -M_PI;
|
||||
if (U > M_PI) U = M_PI;
|
||||
|
||||
if (U > L + EPS) {
|
||||
joint_ranges.emplace_back(L, U);
|
||||
}
|
||||
};
|
||||
|
||||
if (pU_unwrapped <= M_PI + EPS) {
|
||||
// 4) 不跨 +π：单段 [pL, pU_unwrapped]
|
||||
push_if_valid(pL, pU_unwrapped);
|
||||
} else {
|
||||
// 5) 跨 +π 一次：
|
||||
// 第一段： [pL, π]
|
||||
// 第二段： [-π, pU_unwrapped - 2π]
|
||||
push_if_valid(pL, M_PI);
|
||||
|
||||
const double pU2 = pU_unwrapped - TWO_PI; // 回卷到 [-π, π]
|
||||
push_if_valid(-M_PI, pU2);
|
||||
}
|
||||
|
||||
return joint_ranges;
|
||||
}
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_tan_limits(
|
||||
double an, double ad, double bn, double bd, double cn, double cd, double joint_l, double joint_u, double offset) {
|
||||
auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
|
||||
@ -362,11 +530,6 @@ std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_cos_limits(
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// --- 前向预测（给 ψ 预测 θ̂，用于候选打分） ---
|
||||
inline double JointsLimitAnalyzer::predict_theta_tan(
|
||||
double an, double ad, double bn, double bd, double cn, double cd, double psi, double offset) {
|
||||
@ -477,7 +640,7 @@ JointsLimitAnalyzer::estimate_psi_from_joints(
|
||||
// if (cands.empty()) { res.ok=false; return res; }
|
||||
// 认为设置
|
||||
if (cands.empty()) {
|
||||
const int K = 720; // 0.5° 网格
|
||||
const int K = 360; // 1.0° 网格
|
||||
cands.reserve(K);
|
||||
for (int i = 0; i < K; ++i) {
|
||||
cands.push_back(-M_PI + (2.0 * M_PI) * (i + 0.5) / K);
|
||||
371
src/ik_solver/src/opt_psi_limit_bias_solver.cpp
Normal file
371
src/ik_solver/src/opt_psi_limit_bias_solver.cpp
Normal file
@ -0,0 +1,371 @@
|
||||
//
|
||||
// Created by lgv on 11/7/25.
|
||||
//
|
||||
#include "ik_solver/include/opt_psi_limit_bias_solver.h"
|
||||
|
||||
#include "common/utils/math/support_functions.h"
|
||||
|
||||
using namespace cmvr;
|
||||
|
||||
OptPsiLimitBiasSolver::OptPsiLimitBiasSolver() : IKSolver() {
|
||||
bias_srs_ik_solver_ = std::make_shared<BiasSRSIkSolver>();
|
||||
joints_limit_analyzer_ = std::make_shared<JointsLimitAnalyzer>();
|
||||
opt_psi_selector_ = std::make_shared<OptPsiSelector>();
|
||||
|
||||
this->init();
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::init() {
|
||||
Eigen::Matrix4d T_tool_flange, T_arm_robot;
|
||||
|
||||
T_tool_flange << 0, 1, 0, -0.284077,
|
||||
0, 0, 1, 0.00801525,
|
||||
1, 0, 0, 0.00684256,
|
||||
0, 0, 0, 1;
|
||||
|
||||
T_arm_robot << 0, 1, 0, 0,
|
||||
0, 0, -1, 0,
|
||||
-1, 0, 0, 0.042,
|
||||
0, 0, 0, 1;
|
||||
|
||||
T_flange_urdf_mdh_ << 0, 1, 0, 0,
|
||||
0, 0, 1, 0,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0, 1;
|
||||
|
||||
setTcpTransform(T_tool_flange);
|
||||
setArmBaseTransform(T_arm_robot);
|
||||
|
||||
// 臂角更新参数
|
||||
opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4);
|
||||
|
||||
// 代价参数（可之后再通过 set_cost_params 调整）
|
||||
set_cost_params(
|
||||
5.5, // lambda_q_
|
||||
1e-3 // cur_branch_cost_threshold_
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::fk(const std::vector<double> &joints_angle,
|
||||
Eigen::Matrix4d &cur_pose,
|
||||
bool is_tcp) {
|
||||
cur_pose = T_arm_robot_ * bias_srs_ik_solver_->calc_total_transform(joints_angle);
|
||||
if (is_tcp) {
|
||||
cur_pose = cur_pose * T_tool_flange_;
|
||||
} else {
|
||||
cur_pose = cur_pose * T_flange_urdf_mdh_;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::estimate_state_from_current_joints() {
|
||||
if (cur_joints_angle_.size() != 7) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 1) 当前位姿 & 系数矩阵
|
||||
Eigen::Matrix4d cur_pose =
|
||||
bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
|
||||
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
bias_srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat);
|
||||
|
||||
// 2) 清空所有分支的缓存
|
||||
for (int i = 0; i < 2; ++i)
|
||||
for (int j = 0; j < 2; ++j)
|
||||
for (int k = 0; k < 2; ++k)
|
||||
branch_init_[i][j][k].valid = false;
|
||||
|
||||
BranchPsiState best;
|
||||
best.valid = false;
|
||||
double best_score = std::numeric_limits<double>::infinity();
|
||||
|
||||
// 3) 枚举所有 (s,e,w) 分支组合
|
||||
BiasSRSIkSolver::ConfigDirection dirs[2] = {
|
||||
BiasSRSIkSolver::ConfigDirection::OUTWARD,
|
||||
BiasSRSIkSolver::ConfigDirection::INWARD
|
||||
};
|
||||
|
||||
for (BiasSRSIkSolver::ConfigDirection s_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection e_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection w_dir : dirs) {
|
||||
|
||||
int s = sign_from_dir(s_dir);
|
||||
int e = sign_from_dir(e_dir);
|
||||
int w = sign_from_dir(w_dir);
|
||||
|
||||
auto res = joints_limit_analyzer_->estimate_psi_from_joints(
|
||||
s_mat, w_mat,
|
||||
cur_joints_angle_,
|
||||
s, e, w,
|
||||
/*prefer_psi=*/0.0
|
||||
);
|
||||
if (!res.ok) continue;
|
||||
|
||||
// 写入 branch_init_[s,e,w]
|
||||
int is = idx_from_sign(s);
|
||||
int ie = idx_from_sign(e);
|
||||
int iw = idx_from_sign(w);
|
||||
BranchPsiState &slot = branch_init_[is][ie][iw];
|
||||
slot.valid = true;
|
||||
slot.psi = res.psi;
|
||||
slot.s_conf = s;
|
||||
slot.e_conf = e;
|
||||
slot.w_conf = w;
|
||||
|
||||
// 同时更新“最佳分支”作为 branch_state_
|
||||
if (!best.valid || res.score < best_score) {
|
||||
best.valid = true;
|
||||
best.psi = res.psi;
|
||||
best.s_conf = s;
|
||||
best.e_conf = e;
|
||||
best.w_conf = w;
|
||||
best_score = res.score;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!best.valid) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 4) 把求得的“最佳分支”应用到 bias_srs_ik_solver_，保持一致
|
||||
bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(best.s_conf));
|
||||
bias_srs_ik_solver_->set_elbow_config (dir_from_sign(best.e_conf));
|
||||
bias_srs_ik_solver_->set_wrist_config (dir_from_sign(best.w_conf));
|
||||
|
||||
branch_state_ = best;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_pose,
|
||||
const Eigen::MatrixXd &s_mat,
|
||||
const Eigen::MatrixXd &w_mat,
|
||||
const BranchPsiState &branch,
|
||||
std::vector<double> &q_out,
|
||||
double &psi_out,
|
||||
double &cost_out)
|
||||
{
|
||||
auto joints_limits = bias_srs_ik_solver_->get_joints_limits();
|
||||
|
||||
// 1) 这一分支下的 ψ 可行区间
|
||||
auto limits = joints_limit_analyzer_->calc_arm_angle_limits(
|
||||
s_mat, w_mat,
|
||||
joints_limits,
|
||||
branch.s_conf,
|
||||
branch.e_conf,
|
||||
branch.w_conf
|
||||
);
|
||||
if (limits.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 2) 在 limits 中选一个最接近 branch.psi 的 ψ
|
||||
double best_psi = 0.0;
|
||||
if (!opt_psi_selector_->update_psi(best_psi, branch.psi, limits)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3) 把 bias_srs_ik_solver_ 的分支设置为当前 branch
|
||||
bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch.s_conf));
|
||||
bias_srs_ik_solver_->set_elbow_config (dir_from_sign(branch.e_conf));
|
||||
bias_srs_ik_solver_->set_wrist_config (dir_from_sign(branch.w_conf));
|
||||
|
||||
// 4) 解析 IK
|
||||
std::vector<double> q =
|
||||
bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, best_psi);
|
||||
|
||||
if (q.size() != joints_limits.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 5) 检查关节限位
|
||||
for (int i = 0; i < static_cast<int>(q.size()); ++i) {
|
||||
if (q[i] < joints_limits[i].first || q[i] > joints_limits[i].second) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 6) 代价：cost = (Δψ)^2 + lambda_q_ * Σ(Δq_i)^2
|
||||
double dpsi = SupportFunctions::normalize_angle(best_psi - branch.psi);
|
||||
|
||||
double q_cost = 0.0;
|
||||
for (int i = 0; i < static_cast<int>(q.size()); ++i) {
|
||||
double dq = q[i] - cur_joints_angle_[i];
|
||||
q_cost += dq * dq;
|
||||
}
|
||||
cost_out = dpsi * dpsi + lambda_q_ * q_cost;
|
||||
|
||||
q_out = std::move(q);
|
||||
psi_out = best_psi;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
|
||||
std::vector<double> &joints_angle,
|
||||
bool is_tcp)
|
||||
{
|
||||
// --- 0) 预处理 target_cal_pose ---
|
||||
Eigen::Matrix4d target_cal_pose =
|
||||
SupportFunctions::invertHomogeneous(T_arm_robot_) * target_pose;
|
||||
|
||||
if (is_tcp) {
|
||||
target_cal_pose *= SupportFunctions::invertHomogeneous(T_tool_flange_);
|
||||
} else {
|
||||
target_cal_pose *= SupportFunctions::invertHomogeneous(T_flange_urdf_mdh_);
|
||||
}
|
||||
|
||||
// --- 1) 校验当前缓存的 branch_state_ 是否仍然对应 cur_joints_angle_ ---
|
||||
if (branch_state_.valid) {
|
||||
Eigen::Matrix4d cur_pose =
|
||||
bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
|
||||
|
||||
// 确保 solver 内部分支与 branch_state_ 一致
|
||||
bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
|
||||
bias_srs_ik_solver_->set_elbow_config (dir_from_sign(branch_state_.e_conf));
|
||||
bias_srs_ik_solver_->set_wrist_config (dir_from_sign(branch_state_.w_conf));
|
||||
|
||||
auto cur_joints_angle =
|
||||
bias_srs_ik_solver_->inverse_kinematics(cur_pose,
|
||||
branch_state_.psi);
|
||||
|
||||
if (cur_joints_angle.size() != cur_joints_angle_.size()) {
|
||||
branch_state_.valid = false;
|
||||
} else {
|
||||
for (int i = 0; i < static_cast<int>(cur_joints_angle_.size()); ++i) {
|
||||
if (std::abs(cur_joints_angle[i] - cur_joints_angle_[i]) > 1e-3) {
|
||||
branch_state_.valid = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --- 2) 如果还没有“上一次 ψ + 分支”，用当前关节估一次 ---
|
||||
if (!branch_state_.valid) {
|
||||
if (!estimate_state_from_current_joints()) {
|
||||
return false; // 当前姿态都反推不了分支，直接失败
|
||||
}
|
||||
}
|
||||
|
||||
// --- 3) 只算一次 target 位姿的系数矩阵 ---
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
bias_srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat);
|
||||
|
||||
// --- 4) 先在“当前分支”上试一次 ---
|
||||
std::vector<double> q_cur;
|
||||
double psi_cur = 0.0;
|
||||
double cost_cur = 0.0;
|
||||
bool cur_ok = solve_on_branch(target_cal_pose, s_mat, w_mat,
|
||||
branch_state_, q_cur, psi_cur, cost_cur);
|
||||
|
||||
// 若当前分支有解且 cost 足够小，直接用当前分支，不再搜索其它分支
|
||||
if (cur_ok && cost_cur < cur_branch_cost_threshold_) {
|
||||
joints_angle = q_cur;
|
||||
cur_joints_angle_ = joints_angle;
|
||||
branch_state_.psi = psi_cur;
|
||||
branch_state_.valid = true;
|
||||
// s_conf/e_conf/w_conf 不变
|
||||
return true;
|
||||
}
|
||||
|
||||
// --- 5) 当前分支不合适：在 8 个分支中全局搜索最小 cost ---
|
||||
BranchPsiState best_state{};
|
||||
bool have_candidate = false;
|
||||
double best_cost = std::numeric_limits<double>::infinity();
|
||||
std::vector<double> best_q;
|
||||
|
||||
// 先把“当前分支的候选”也纳入考虑，避免再算一遍
|
||||
if (cur_ok) {
|
||||
have_candidate = true;
|
||||
best_cost = cost_cur;
|
||||
best_q = q_cur;
|
||||
|
||||
best_state = branch_state_;
|
||||
best_state.psi = psi_cur;
|
||||
best_state.valid = true;
|
||||
}
|
||||
|
||||
BiasSRSIkSolver::ConfigDirection dirs[2] = {
|
||||
BiasSRSIkSolver::ConfigDirection::OUTWARD,
|
||||
BiasSRSIkSolver::ConfigDirection::INWARD
|
||||
};
|
||||
|
||||
for (BiasSRSIkSolver::ConfigDirection s_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection e_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection w_dir : dirs) {
|
||||
|
||||
int s = sign_from_dir(s_dir);
|
||||
int e = sign_from_dir(e_dir);
|
||||
int w = sign_from_dir(w_dir);
|
||||
|
||||
// 当前分支已经作为候选，跳过
|
||||
if (s == branch_state_.s_conf &&
|
||||
e == branch_state_.e_conf &&
|
||||
w == branch_state_.w_conf) {
|
||||
continue;
|
||||
}
|
||||
|
||||
BranchPsiState cand{};
|
||||
cand.s_conf = s;
|
||||
cand.e_conf = e;
|
||||
cand.w_conf = w;
|
||||
|
||||
// 为这一分支选择它自己的 prefer_psi：
|
||||
if (const BranchPsiState* init_st = get_branch_init_state(s, e, w)) {
|
||||
cand.psi = init_st->psi; // 用该分支自己的 ψ 估计
|
||||
} else {
|
||||
cand.psi = branch_state_.psi; // 兜底：用当前主分支的 ψ
|
||||
}
|
||||
|
||||
std::vector<double> q_cand;
|
||||
double psi_cand = 0.0;
|
||||
double cost_cand = 0.0;
|
||||
|
||||
if (!solve_on_branch(target_cal_pose, s_mat, w_mat,
|
||||
cand, q_cand, psi_cand, cost_cand)) {
|
||||
continue; // 这一分支无解
|
||||
}
|
||||
|
||||
// 是否与上一帧处于同一分支，用来做 tie-break
|
||||
bool same_branch =
|
||||
(s == branch_state_.s_conf &&
|
||||
e == branch_state_.e_conf &&
|
||||
w == branch_state_.w_conf);
|
||||
|
||||
if (!have_candidate ||
|
||||
cost_cand < best_cost - 1e-12 ||
|
||||
(std::abs(cost_cand - best_cost) <= 1e-12 && same_branch))
|
||||
{
|
||||
have_candidate = true;
|
||||
best_cost = cost_cand;
|
||||
best_q = std::move(q_cand);
|
||||
|
||||
best_state = cand;
|
||||
best_state.psi = psi_cand;
|
||||
best_state.valid = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!have_candidate) {
|
||||
// 所有分支都无解 → 目标在当前关节限位下真不可达
|
||||
return false;
|
||||
}
|
||||
|
||||
// --- 6) 用最佳分支 + ψ 更新状态并返回 ---
|
||||
branch_state_ = best_state;
|
||||
|
||||
bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
|
||||
bias_srs_ik_solver_->set_elbow_config (dir_from_sign(branch_state_.e_conf));
|
||||
bias_srs_ik_solver_->set_wrist_config (dir_from_sign(branch_state_.w_conf));
|
||||
|
||||
joints_angle = best_q;
|
||||
cur_joints_angle_ = joints_angle; // 作为下一次的“当前姿态”
|
||||
|
||||
return true;
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user