forked from galou/freecad-symoro
-
Notifications
You must be signed in to change notification settings - Fork 0
/
serialmechanism.py
executable file
·48 lines (41 loc) · 2.21 KB
/
serialmechanism.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
#***************************************************************************
#* *
#* Copyright (c) 2012 Gael Ecorchard <galou_breizh@yahoo.fr> *
#* *
#* This program is free software; you can redistribute it and/or modify *
#* it under the terms of the GNU Lesser General Public License (LGPL) *
#* as published by the Free Software Foundation; either version 2 of *
#* the License, or (at your option) any later version. *
#* for detail see the LICENCE text file. *
#* *
#* This program is distributed in the hope that it will be useful, *
#* but WITHOUT ANY WARRANTY; without even the implied warranty of *
#* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
#* GNU Library General Public License for more details. *
#* *
#* You should have received a copy of the GNU Library General Public *
#* License along with this program; if not, write to the Free Software *
#* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 *
#* USA *
#* *
#***************************************************************************
__title__ = "FreeCAD Symoro+ Workbench - SerialMechanism"
__author__ = "Gael Ecorchard <galou_breizh@yahoo.fr>"
__url__ = ["http://free-cad.sourceforge.net"]
import numpy.matlib as matlib
def n_pjoints(joints):
"""Return the number of passive joints"""
n = 0
for jnt in joints:
if jnt.ispassive():
n += 1
return n
def end_transform(joints):
"""Return the homogeneous transformation matrix of the whole serial chain
Return the homogeneous transformation matrix of the whole serial chain as
numpy.matrix.
"""
T = matlib.identity(4)
for jnt in joints:
T *= jnt.T
return T