Fixed domain errors in acos near identity · rai-opensource/spatialmath-python@5d1044a · GitHub

rai-opensource / spatialmath-python Public

• Notifications You must be signed in to change notification settings
• Fork 102
• Star 642

• Code
• Issues 10
• Pull requests 23
• Discussions
• Actions
• Wiki
• Security and quality
• Insights

Additional navigation options

Commit 5d1044a

Browse filesBrowse the repository at this point in the historyBrowse files

bokorn-bdaii

committed

Fixed domain errors in acos near identity

1 parent 469b5fa commit 5d1044a
Copy full SHA for 5d1044a

2 files changed

+105-109Lines changed: 105 additions & 109 deletions

File tree

Expand file treeCollapse file tree

Open diff view settings

Filter options

• spatialmath/base
  • transforms3d.py
• tests
  • test_pose3d.py

Expand file treeCollapse file tree

Open diff view settings

Collapse file

‎spatialmath/base/transforms3d.py‎

Copy file name to clipboardExpand all lines: spatialmath/base/transforms3d.py

+3-1Lines changed: 3 additions & 1 deletion

Original file line number	Diff line number	Diff line change
@@ -1376,7 +1376,9 @@ def trlog(
1376	1376	return skew(w * theta)
1377	1377	else:
1378	1378	# general case
1379		- theta = math.acos((np.trace(R) - 1) / 2)
	1379	+ tr = (np.trace(R) - 1) / 2
	1380	+ # min for inaccuracies near identity yielding trace > 3
	1381	+ theta = math.acos(min(tr, 1.0))
1380	1382	st = math.sin(theta)
1381	1383	if st == 0:
1382	1384	if twist:

0 commit comments

Comments

0 (0)