Skip to content

Commit 58d99ca

Browse files
jhavljhavl
committed
finished ik_lm_chan
1 parent 60e817a commit 58d99ca

File tree

5 files changed

+272
-188
lines changed

5 files changed

+272
-188
lines changed

‎roboticstoolbox/core/fknm.cpp

Lines changed: 35 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99

1010
#include "fknm.h"
1111
#include "methods.h"
12+
#include "ik.h"
1213
#include "linalg.h"
1314
#include "structs.h"
1415

@@ -19,8 +20,8 @@
1920
#include <iostream>
2021

2122
static PyMethodDef fknmMethods[] = {
22-
{"IK",
23-
(PyCFunction)IK,
23+
{"IK_lm_chan",
24+
(PyCFunction)IK_lm_chan,
2425
METH_VARARGS,
2526
"Link"},
2627
{"Robot_link_T",
@@ -86,28 +87,32 @@ PyMODINIT_FUNC PyInit_fknm(void)
8687

8788
extern "C"
8889
{
89-
static PyObject *IK(PyObject *self, PyObject *args)
90+
91+
92+
static PyObject *IK_lm_chan(PyObject *self, PyObject *args)
9093
{
9194
ETS *ets;
92-
npy_float64 *np_Tep, *np_ret, *np_q0;
95+
npy_float64 *np_Tep, *np_ret, *np_q0, *np_we;
9396
PyArrayObject *py_np_Tep;
94-
PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0;
97+
PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0, *py_we, *py_np_we;
9598
PyObject *py_tup, *py_it, *py_search, *py_solution, *py_E;
9699
npy_intp dim[1] = {1};
97-
int ilimit, slimit, q0_used = 0, reject_jl;
98-
double tol, E;
100+
int ilimit, slimit, q0_used = 0, we_used = 0, reject_jl;
101+
double tol, E, lambda;
99102

100103
int it = 0, search = 0, solution = 0;
101104

102105
if (!PyArg_ParseTuple(
103-
args, "OOOiidi",
106+
args, "OOOiidiOd",
104107
&py_ets,
105108
&py_Tep,
106109
&py_q0,
107110
&ilimit,
108111
&slimit,
109112
&tol,
110-
&reject_jl))
113+
&reject_jl,
114+
&py_we,
115+
&lambda))
111116
return NULL;
112117

113118
if (!_check_array_type(py_Tep))
@@ -117,8 +122,9 @@ extern "C"
117122
if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
118123
return NULL;
119124

120-
// Assign empty q0
125+
// Assign empty q0 and we
121126
MapVectorX q0(NULL, 0);
127+
MapVectorX we(NULL, 0);
122128

123129
// Check if q0 is None
124130
if (py_q0 != Py_None)
@@ -135,7 +141,19 @@ extern "C"
135141
new (&q0) MapVectorX(np_q0, ets->n);
136142
}
137143

138-
// std::cout << q0.size() << '\n';
144+
// Check if we is None
145+
if (py_we != Py_None)
146+
{
147+
// Make sure we is number array
148+
// Cast to numpy array
149+
// Get data out
150+
if (!_check_array_type(py_we))
151+
return NULL;
152+
we_used = 1;
153+
py_np_we = (PyObject *)PyArray_FROMANY(py_we, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
154+
np_we = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_we);
155+
new (&we) MapVectorX(np_we, 6);
156+
}
139157

140158
// Set the dimension of the returned array to match the number of joints
141159
dim[0] = ets->n;
@@ -156,9 +174,7 @@ extern "C"
156174
// std::cout << Tep << std::endl;
157175
// std::cout << ret << std::endl;
158176

159-
_IK_LM_Chan(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E);
160-
161-
// _angle_axis(Te, Tep, ret);
177+
_IK_LM_Chan(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we);
162178

163179
// Free the memory
164180
Py_DECREF(py_np_Tep);
@@ -168,8 +184,12 @@ extern "C"
168184
Py_DECREF(py_np_q0);
169185
}
170186

171-
// Build the return tuple
187+
if (we_used)
188+
{
189+
Py_DECREF(py_np_we);
190+
}
172191

192+
// Build the return tuple
173193
py_it = Py_BuildValue("i", it);
174194
py_search = Py_BuildValue("i", search);
175195
py_solution = Py_BuildValue("i", solution);

‎roboticstoolbox/core/ik.cpp

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
1+
/* ik.cpp */
2+
3+
#include "linalg.h"
4+
#include "methods.h"
5+
#include "ik.h"
6+
#include "structs.h"
7+
8+
#include <Python.h>
9+
#include <math.h>
10+
#include <iostream>
11+
#include <Eigen/Dense>
12+
#include <Eigen/QR>
13+
14+
extern "C"
15+
{
16+
17+
void _IK_LM_Chan(
18+
ETS *ets, Matrix4dc Tep,
19+
MapVectorX q0, int ilimit, int slimit, double tol, int reject_jl,
20+
MapVectorX q, int *it, int *search, int *solution, double *E,
21+
double lambda, MapVectorX we)
22+
{
23+
int iter = 0;
24+
25+
double *np_Te = (double *)PyMem_RawCalloc(16, sizeof(double));
26+
MapMatrix4dc Te(np_Te);
27+
28+
double *np_J = (double *)PyMem_RawCalloc(6 * ets->n, sizeof(double));
29+
MapMatrixJc J(np_J, 6, ets->n);
30+
31+
double *np_e = (double *)PyMem_RawCalloc(6, sizeof(double));
32+
MapVectorX e(np_e, 6);
33+
34+
Matrix6dc We;
35+
36+
Eigen::MatrixXd Wn(ets->n, ets->n);
37+
Eigen::MatrixXd EyeN = Eigen::MatrixXd::Identity(ets->n, ets->n);
38+
39+
VectorX g(ets->n);
40+
41+
// Set we
42+
if (we.size() == 6)
43+
{
44+
We = we.asDiagonal();
45+
}
46+
else
47+
{
48+
We = Matrix6dc::Identity();
49+
}
50+
51+
// Set the first q0
52+
if (q0.size() == ets->n)
53+
{
54+
q = q0;
55+
}
56+
else
57+
{
58+
q = _rand_q(ets);
59+
}
60+
61+
// Global search up to slimit
62+
while (*search < slimit)
63+
{
64+
65+
while (iter < ilimit)
66+
{
67+
// Current pose Te
68+
_ETS_fkine(ets, q.data(), (double *)NULL, NULL, Te);
69+
70+
// Angle axis error e
71+
_angle_axis(Te, Tep, e);
72+
73+
// Squared error E
74+
*E = 0.5 * e.transpose() * We * e;
75+
76+
if (*E < tol)
77+
{
78+
// We have arrived
79+
80+
// Check for joint limit violation
81+
if (reject_jl)
82+
{
83+
*solution = _check_lim(ets, q);
84+
}
85+
else
86+
{
87+
*solution = 1;
88+
}
89+
90+
break;
91+
}
92+
93+
// Jacobian Matric J
94+
_ETS_jacob0(ets, q.data(), (double *)NULL, J);
95+
96+
// Weighting matrix Wn
97+
Wn = lambda * *E * EyeN;
98+
99+
// The vector g
100+
g = J.transpose() * We * e;
101+
102+
// Work out the joint velocity qd
103+
q += (J.transpose() * We * J + Wn).inverse() * g;
104+
105+
iter += 1;
106+
}
107+
108+
if (*solution)
109+
{
110+
*it += iter;
111+
break;
112+
}
113+
114+
*it += iter;
115+
iter = 0;
116+
*search += 1;
117+
q = _rand_q(ets);
118+
}
119+
120+
free(np_e);
121+
free(np_Te);
122+
free(np_J);
123+
}
124+
125+
int _check_lim(ETS *ets, MapVectorX q)
126+
{
127+
for (int i = 0; i < ets->n; i++)
128+
{
129+
if (q(i) < ets->qlim_l[i] || q(i) > ets->qlim_h[i])
130+
{
131+
// std::cout << "Joint limit: " << q.transpose() << " : " << q(i) << "\n";
132+
return 0;
133+
}
134+
}
135+
136+
return 1;
137+
}
138+
139+
void _angle_axis(MapMatrix4dc Te, Matrix4dc Tep, MapVectorX e)
140+
{
141+
double num, li_norm, R_tr, ang;
142+
Matrix3dc R;
143+
Vector3 li;
144+
145+
// e[:3] = Tep[:3, 3] - Te[:3, 3]
146+
e.block<3, 1>(0, 0) = Tep.block<3, 1>(0, 3) - Te.block<3, 1>(0, 3);
147+
148+
// R = Tep.R @ T.R.T
149+
// R = Tep[:3, :3] @ T[:3, :3].T
150+
R = Tep.block<3, 3>(0, 0) * Te.block<3, 3>(0, 0).transpose();
151+
152+
// li = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]);
153+
li << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1);
154+
155+
// if base.iszerovec(li)
156+
li_norm = li.norm();
157+
158+
R_tr = R.trace();
159+
if (li_norm < 1e-6)
160+
{
161+
// diagonal matrix case
162+
// if np.trace(R) > 0
163+
if (R_tr > 0)
164+
{
165+
// (1,1,1) case
166+
// a = np.zeros((3, ));
167+
e.block<3, 1>(3, 0) << 0, 0, 0;
168+
}
169+
else
170+
{
171+
// a = np.pi / 2 * (np.diag(R) + 1);
172+
e(3) = PI_2 * (R(0, 0) + 1);
173+
e(4) = PI_2 * (R(1, 1) + 1);
174+
e(5) = PI_2 * (R(2, 2) + 1);
175+
}
176+
}
177+
else
178+
{
179+
// non-diagonal matrix case
180+
// a = math.atan2(li_norm, np.trace(R) - 1) * li / li_norm
181+
ang = atan2(li_norm, R_tr - 1);
182+
e.block<3, 1>(3, 0) = ang * li / li_norm;
183+
}
184+
}
185+
186+
VectorX _rand_q(ETS *ets)
187+
{
188+
Eigen::Map<Eigen::ArrayXd> qlim_l(ets->qlim_l, ets->n);
189+
Eigen::Map<Eigen::ArrayXd> q_range2(ets->q_range2, ets->n);
190+
191+
VectorX q = VectorX::Random(ets->n);
192+
193+
q = (q.array() + 1) * q_range2;
194+
q = q.array() + qlim_l;
195+
196+
return q;
197+
}
198+
199+
} /* extern "C" */

‎roboticstoolbox/core/ik.h

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
/**
2+
* \file ik.h
3+
* \author Jesse Haviland
4+
*
5+
*/
6+
/* ik.h */
7+
8+
#ifndef _IK_H_
9+
#define _IK_H_
10+
11+
#include <Python.h>
12+
#include "structs.h"
13+
#include "linalg.h"
14+
15+
#ifdef __cplusplus
16+
extern "C"
17+
{
18+
#endif /* __cplusplus */
19+
20+
void _IK_LM_Chan(
21+
ETS *ets, Matrix4dc Tep,
22+
MapVectorX q0, int ilimit, int slimit, double tol, int reject_jl,
23+
MapVectorX q, int *it, int *search, int *solution, double *E,
24+
double lambda, MapVectorX we
25+
);
26+
27+
VectorX _rand_q(ETS *ets);
28+
int _check_lim(ETS *ets, MapVectorX q);
29+
void _angle_axis(MapMatrix4dc Te, Matrix4dc Tep, MapVectorX e);
30+
31+
#ifdef __cplusplus
32+
} /* extern "C" */
33+
#endif /* __cplusplus */
34+
35+
#endif

0 commit comments

Comments
 (0)