xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修正沿法向量方向运动问题

Browse Source
This commit is contained in:
cdeyw
2025-01-16 12:10:36 +08:00
parent ecfd9551e1
commit aa9fd3469b
9 changed files with 123 additions and 1566 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.idea/.gitignore generated vendored Normal file
View File

@ -0,0 +1,3 @@
# 默认忽略的文件
/shelf/
/workspace.xml

12
.idea/AutoControlSystem.iml generated Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="rob" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">
<option name="format" value="PLAIN" />
<option name="myDocStringFormat" value="Plain" />
</component>
</module>

6
.idea/inspectionProfiles/profiles_settings.xml generated Normal file
View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

7
.idea/misc.xml generated Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Black">
<option name="sdkName" value="rob" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="rob" project-jdk-type="Python SDK" />
</project>

8
.idea/modules.xml generated Normal file
View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/AutoControlSystem.iml" filepath="$PROJECT_DIR$/.idea/AutoControlSystem.iml" />
</modules>
</component>
</project>

6
.idea/vcs.xml generated Normal file
View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

10
Trace/handeye_calibration.py
View File

@ -54,12 +54,12 @@ def R_matrix(x,y,z,u,v,w):
# 图像识别结果：xyz和法向量
def getPosition(x,y,z,a,b,c,rotation,points):
def getPosition(x,y,z,a,b,c,points):
target = np.asarray([x, y, z,1])
camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ') #相对目录且分隔符采用os.sep
# robot2base = rotation
# camera2base = robot2base @ camera2robot
target_position = np.dot(camera2robot, target)
target_position_raw = np.dot(camera2robot, target)
corner_points_camera = np.asarray(points)
corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
@ -76,8 +76,14 @@ def getPosition(x,y,z,a,b,c,rotation,points):
# 单位化方向向量
short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
delta = -30#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
angle = np.asarray([a,b,c])
noraml = camera2robot[:3, :3]@angle
normal_vector = noraml / np.linalg.norm(noraml)
target_position = target_position_raw[:3] + delta * normal_vector # target_position 沿法向量移动
noraml_base = vec2rpy(noraml,short_edge_direction)
return target_position,noraml_base

1597
log/log.log
View File

File diff suppressed because it is too large Load Diff

40
log/log.log.2024-12-14 Normal file
View File

@ -0,0 +1,40 @@
2024-12-14 16:10:42 - INFO - <20><><EFBFBD><EFBFBD>ϵͳ
2024-12-14 16:11:15 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:14:02 - INFO - <20>ƶ<EFBFBD><C6B6><EFBFBD>λ<EFBFBD><CEBB>:<3A><><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD><EFBFBD>m0:1.0-m2:0.0-m3:0.0-m4:0.0-m5:0.0-m6:0.0
2024-12-14 16:14:06 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:14:10 - INFO - <20><><EFBFBD><EFBFBD>Ͷ<EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD>:1
2024-12-14 16:14:11 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:14:11 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>״̬
2024-12-14 16:14:12 - INFO - <20><>һ<EFBFBD><D2BB>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:14:12 - INFO - <20><>Ӧ<EFBFBD><D3A6>1:Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:14:12 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>1
2024-12-14 16:14:14 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:14:17 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>0
2024-12-14 16:14:17 - INFO - Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:14:21 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:21:15 - INFO - <20>˳<EFBFBD>ϵͳ
2024-12-14 16:21:15 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:21:15 - ERROR - model
2024-12-14 16:21:27 - INFO - <20><><EFBFBD><EFBFBD>ϵͳ
2024-12-14 16:21:59 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:37:33 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:37:33 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>״̬
2024-12-14 16:37:34 - INFO - <20><>һ<EFBFBD><D2BB>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:37:34 - INFO - <20><>Ӧ<EFBFBD><D3A6>1:Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:37:34 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>1
2024-12-14 16:37:38 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:37:39 - INFO - <20><><EFBFBD><EFBFBD>Ͷ<EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD>:10
2024-12-14 16:37:39 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:37:39 - INFO - <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>״̬
2024-12-14 16:37:39 - INFO - <20><><EFBFBD><EFBFBD>IO<49><4F><EFBFBD><EFBFBD>: 3<><33>0
2024-12-14 16:37:39 - INFO - Ͷ<>Ͽ<EFBFBD>ʼ
2024-12-14 16:37:46 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:39:27 - INFO - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:39:27 - INFO - Ͷ<><CDB6>ֹͣ
2024-12-14 16:39:28 - INFO - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2024-12-14 16:39:28 - INFO - Ͷ<><CDB6>ֹͣ
2024-12-14 16:39:30 - INFO - Ͷ<><CDB6><EFBFBD><EFBFBD>ͣ
2024-12-14 16:39:30 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:39:30 - INFO - Ͷ<><CDB6><EFBFBD><EFBFBD>ͣ
2024-12-14 16:39:38 - ERROR - <20>޻ظ<DEBB>
2024-12-14 16:41:47 - INFO - <20>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>