Blender  V3.3
ConstraintSet.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Ruben Smits. */
3 
8 #include "ConstraintSet.hpp"
10 
11 namespace iTaSC {
12 
13 ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations):
14  m_nc(_nc),
15  m_Cf(e_zero_matrix(m_nc,6)),
16  m_Wy(e_scalar_vector(m_nc,1.0)),
17  m_y(m_nc),m_ydot(e_zero_vector(m_nc)),m_chi(e_zero_vector(6)),
18  m_S(6),m_temp(6),m_tdelta(6),
19  m_Jf(e_identity_matrix(6,6)),
20  m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)),
21  m_Jf_inv(e_zero_matrix(6,6)),
22  m_internalPose(F_identity), m_externalPose(F_identity),
23  m_constraintCallback(NULL), m_constraintParam(NULL),
24  m_toggle(false),m_substep(false),
25  m_threshold(accuracy),m_maxIter(maximum_iterations)
26 {
27  m_maxDeltaChi = e_scalar(0.52);
28 }
29 
31  m_nc(0),
32  m_internalPose(F_identity), m_externalPose(F_identity),
33  m_constraintCallback(NULL), m_constraintParam(NULL),
34  m_toggle(false),m_substep(false),
35  m_threshold(0.0),m_maxIter(0)
36 {
37  m_maxDeltaChi = e_scalar(0.52);
38 }
39 
40 void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)
41 {
42  m_nc = _nc;
43  m_Jf = e_identity_matrix(6,6);
44  m_Cf = e_zero_matrix(m_nc,6);
45  m_U = e_identity_matrix(6,6);
46  m_V = e_identity_matrix(6,6);
47  m_B = e_zero_matrix(6,6);
48  m_Jf_inv = e_zero_matrix(6,6),
49  m_Wy = e_scalar_vector(m_nc,1.0),
50  m_chi = e_zero_vector(6);
54  m_S = e_zero_vector(6);
55  m_temp = e_zero_vector(6);
57  m_threshold = accuracy;
58  m_maxIter = maximum_iterations;
59 }
60 
62 
63 }
64 
65 void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
66 {
67  m_chi+=m_chidot*timestamp.realTimestep;
68  m_externalPose = _external_pose;
69 
70  //update the internal pose and Jf
72  //check if loop is already closed, if not update the pose and Jf
73  unsigned int iter=0;
74  while(iter<5&&!closeLoop())
75  iter++;
76 }
77 
78 double ConstraintSet::getMaxTimestep(double& timestep)
79 {
80  e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
81  if (timestep*maxChidot > m_maxDeltaChi) {
82  timestep = m_maxDeltaChi/maxChidot;
83  }
84  return timestep;
85 }
86 
88  m_externalPose=init_pose;
89  // get current Jf
91 
92  unsigned int iter=0;
93  while(iter<m_maxIter&&!closeLoop()){
94  iter++;
95  }
96  if (iter<m_maxIter)
97  return true;
98  else
99  return false;
100 }
101 
102 bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
103 {
104  ConstraintValues values;
105  ConstraintSingleValue value;
106  values.values = &value;
107  values.number = 0;
108  values.action = action;
109  values.id = id;
110  value.action = action;
111  value.id = id;
112  switch (action) {
113  case ACT_NONE:
114  return true;
115  case ACT_VALUE:
116  value.yd = data;
117  values.number = 1;
118  break;
119  case ACT_VELOCITY:
120  value.yddot = data;
121  values.number = 1;
122  break;
123  case ACT_TOLERANCE:
124  values.tolerance = data;
125  break;
126  case ACT_FEEDBACK:
127  values.feedback = data;
128  break;
129  case ACT_ALPHA:
130  values.alpha = data;
131  break;
132  default:
133  assert(action==ACT_NONE);
134  break;
135  }
136  return setControlParameters(&values, 1, timestep);
137 }
138 
140  //Invert Jf
141  //TODO: svd_boost_Macie has problems if Jf contains zero-rows
142  //toggle=!toggle;
143  //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
145  if(ret<0)
146  return false;
147 
148  // the reference point and frame of the jacobian is the base frame
149  // m_externalPose-m_internalPose is the twist to extend the end effector
150  // to get the required pose => change the reference point to the base frame
151  Twist twist_delta(diff(m_internalPose,m_externalPose));
152  twist_delta=twist_delta.RefPoint(-m_internalPose.p);
153  for(unsigned int i=0;i<6;i++)
154  m_tdelta(i)=twist_delta(i);
155  //TODO: use damping in constraintset inversion?
156  for(unsigned int i=0;i<6;i++) {
157  if(m_S(i)<m_threshold){
158  m_B.row(i).setConstant(0.0);
159  } else {
160  m_B.row(i) = m_U.col(i)/m_S(i);
161  }
162  }
163 
164  m_Jf_inv.noalias()=m_V*m_B;
165 
166  m_chi.noalias()+=m_Jf_inv*m_tdelta;
167  updateJacobian();
168  // m_externalPose-m_internalPose in end effector frame
169  // this is just to compare the pose, a different formula would work too
171 
172 }
173 }
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
Vector p
origine of the Frame
Definition: frames.hpp:528
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:431
represents both translational and rotational velocities.
Definition: frames.hpp:679
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:322
virtual void updateJacobian()=0
bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0)
virtual double getMaxTimestep(double &timestep)
virtual bool initialise(KDL::Frame &init_pose)
unsigned int m_maxIter
virtual void modelUpdate(KDL::Frame &_external_pose, const Timestamp &timestamp)
virtual void reset(unsigned int nc, double accuracy, unsigned int maximum_iterations)
virtual bool closeLoop()
virtual bool setControlParameters(ConstraintValues *_values, unsigned int _nvalues, double timestep=0.0)=0
#define e_scalar_vector
Definition: eigen_types.hpp:43
#define e_scalar
Definition: eigen_types.hpp:37
#define e_zero_vector
Definition: eigen_types.hpp:39
#define e_identity_matrix
Definition: eigen_types.hpp:42
#define e_zero_matrix
Definition: eigen_types.hpp:44
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
int svd_eigen_HH(const Eigen::MatrixBase< MatrixA > &A, Eigen::MatrixBase< MatrixUV > &U, Eigen::MatrixBase< VectorS > &S, Eigen::MatrixBase< MatrixUV > &V, Eigen::MatrixBase< VectorS > &tmp, int maxiter=150)
const Frame F_identity
@ ACT_TOLERANCE
@ ACT_FEEDBACK
@ ACT_VELOCITY
return ret
struct ConstraintSingleValue * values
double realTimestep
Definition: Cache.hpp:37