Go to the documentation of this file.
18 double calc_jacobian_time = 0;
19 double solve_ik_time = 0;
20 double high_constraint_time = 0;
21 double low_constraint_time = 0;
35 calc_jacobian_time += (double)(t2 - t1) / (double)CLOCKS_PER_SEC;
39 solve_ik_time += (double)(clock() - t2) / (
double)CLOCKS_PER_SEC;
46 double IK::Update(
double max_timestep,
double min_timestep,
double max_integ_error)
49 double new_timestep = max_timestep;
50 for(
int i=0;
i<2;
i++)
160 double current_max_condnum = -1.0;
171 int* is_high_const = 0;
172 double cond_number = 1.0;
180 is_high_const[
i] =
true;
182 int search_phase = 0;
188 if(is_high_const[
i]) n_high_const++;
192 fb_high.
resize(n_high_const);
193 weight_high.
resize(n_high_const);
201 for(j=0; j<
n_dof; j++)
216 for(j=0; j<
n_dof; j++)
228 if(n_high_const <
n_dof) s_size = n_high_const;
231 wJhigh.
svd(U, s, VT);
234 cond_number = s(0) / s(s_size-1);
236 cond_number = condnum_limit;
237 if(current_max_condnum < 0.0 || cond_number > current_max_condnum)
239 current_max_condnum = cond_number;
269 if(!reduced) search_phase++;
279 fb_high.
resize(n_high_const);
280 weight_high.
resize(n_high_const);
297 cerr << current_max_condnum <<
", " << k << endl;
301 if(current_max_condnum < 0.0) current_max_condnum = 1.0;
304 int low_first = 0, count = 0;
306 fVec fb_low(n_low_const);
307 fVec weight_low(n_low_const);
310 if(!is_high_const[
i])
314 for(j=0; j<
n_dof; j++)
322 double* p = fb_low.
data() + low_first;
324 double* r = weight_low.
data() + low_first;
331 double*
a = Jlow.
data() + low_first +
i;
332 int a_row = Jlow.
row();
335 for(j=0; j<
n_dof;
a+=a_row,
b+=b_row, j++)
341 if(is_high_const)
delete[] is_high_const;
345 fVec fb_low_0(n_low_const), dfb(n_low_const), y(
n_dof);
347 fVec w_error(n_low_const), w_norm(
n_dof);
349 double damping = 0.1;
351 w_error.set(weight_low);
358 clock_t t1 = clock();
365 int a_row = wJhigh.
row();
366 double*
a = wJhigh.
data() + a_row*
i;
367 int b_row = Jhigh.
row();
368 double*
b = Jhigh.
data() + b_row*
i;
370 double* d = weight_high.
data();
371 for(j=0; j<n_high_const;
a++,
b++, d++, j++)
377 fVec w_fb_high(fb_high);
378 for(
i=0;
i<n_high_const;
i++)
379 w_fb_high(
i) *= weight_high(
i);
381 jvel0.
mul(Jinv, w_fb_high);
385 double* w = W.data() +
i;
402 clock_t t2 = clock();
403 high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC;
408 fb_low_0.mul(Jlow, jvel0);
409 dfb.sub(fb_low, fb_low_0);
410 y.
lineq_sr(JW, w_error, w_norm, damping, dfb);
411 if(n_high_const > 0) jvel.
mul(W, y);
424 clock_t t3 = clock();
425 low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC;
430 if(current_max_condnum < 0.0) current_max_condnum = 1.0;
431 return current_max_condnum;
457 int target_row = targetJ.
row();
469 for(j=0; j<n_dof;
a+=target_row,
b+=
row, j++)
void set(double *_d)
Sets all elements.
fVec joint_weights
joint weights
int lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fVec &b)
The class for representing a joint.
int size() const
Size of the vector (same as row()).
@ HIGH_PRIORITY
strictly satisfied
IKConstraint ** constraints
list of current constraints
int n_constraints
number of current constraints
JointType j_type
joint type
double * data() const
Returns the pointer to the first element.
int IntegrateAdaptive(double ×tep, int step, double min_timestep=DEFAULT_MIN_TIMESTEP, double max_integ_error=DEFAULT_MAX_INTEG_ERROR)
Performs Euler integration with adaptive timestep.
virtual int calc_jacobian()
Computes the constraint Jacobian.
virtual int calc_feedback()=0
compute the feedback velocity
Joint * joint
target joint
double Update(double timestep)
Joint * child
pointer to the child joint
int NumDOF()
Total degrees of freedom.
@ JROTATE
rotational (1DOF)
@ JSPHERE
spherical (3DOF)
void mul(const fVec &vec, double d)
fMat J[N_PRIORITY_TYPES]
Jacobian matrix of each type.
void mul(const fMat &mat1, const fMat &mat2)
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order.
int resize(int i, int j)
Changes the matrix size.
int is_dropped
index in the constraints with the same priority
void zero()
Creates a zero matrix.
void CalcPosition()
Forward kinematics.
int SetJointVel(const fVec &vels)
Set all joint velocities/accelerations.
int enabled
number of constraints
virtual int calc_jacobian_sphere(Joint *cur)
void set(double *_d)
Sets the values from an array.
@ LOW_PRIORITY
lower priority
int DescendantDOF()
Total DOF of the descendants (end link side).
@ HIGH_IF_POSSIBLE
strictly satisfied if possible
virtual int calc_jacobian_free(Joint *cur)
fVec weight[N_PRIORITY_TYPES]
weight of each type
@ HANDLE_CONSTRAINT
position/orientation of link
int svd(fMat &U, fVec &Sigma, fMat &VT)
singular value decomposition
double solve_ik()
compute the joint velocity
virtual int calc_jacobian_slide(Joint *cur)
int n_const[N_PRIORITY_TYPES]
number of constraints of each type
virtual int calc_jacobian_rotate(Joint *cur)
Inverse kinematics (UTPoser) class.
void zero()
Creates a zero vector.
void resize(int i)
Change the size.
int row() const
Returns the number of rows.
int calc_jacobian()
compute the Jacobian matrix
Joint * brother
pointer to the brother joint
int Integrate(double timestep)
Performs Euler integration with timestep timestep [s].
int i_const
feedback velocity (n_const)
fVec fb[N_PRIORITY_TYPES]
constraint error of each type
fVec fb
Jacobian matrix (n_const x total DOF)
int copy_jacobian()
copy each constraint Jacobian to the whole Jacobian matrix
double max_condnum
maximum condition number
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04