20 m_chiKdl(6),m_jac(6),m_cache(
NULL),
21 m_distCCh(-1),m_distCTs(0)
35 m_maxerror = armlength/2.0;
38 m_yddot = m_nextyddot = 0.0;
40 memset(&m_data, 0,
sizeof(m_data));
45 m_values.
alpha = m_alpha;
57 bool Distance::computeChi(
Frame& pose)
59 double dist, alpha,
beta, gamma;
68 Vector axis(pose.
p/dist);
127 void Distance::pushDist(
CacheTS timestamp)
129 if (m_distCCh >= 0) {
133 *item++ = m_tolerance;
139 m_distCTs = timestamp;
143 bool Distance::popDist(
CacheTS timestamp)
145 if (m_distCCh >= 0) {
147 if (item && timestamp != m_distCTs) {
149 m_values.
tolerance = m_tolerance = *item++;
152 m_values.
alpha = m_alpha = *item++;
154 m_distCTs = timestamp;
158 return (item) ? true :
false;
180 m_yddot = m_nextyddot;
188 for(
unsigned int i=0;i<6;i++)
189 m_chiKdl[i]=
m_chi[i];
192 m_jacsolver->
JntToJac(m_chiKdl,m_jac);
194 for(
unsigned int i=0;i<6;i++)
195 for(
unsigned int j=0;j<6;j++)
196 m_Jf(i,j)=m_jac(i,j);
205 while (_nvalues > 0) {
208 m_alpha = _values->
alpha;
219 for (_data = _values->
values, i=0; i<_values->number; i++, _data++) {
227 _data->
yd = m_yd+_data->
yddot*timestep;
233 m_nextyd = _data->
yd;
238 m_yddot = (_data->
yd-m_yd)/timestep;
242 m_yddot = m_nextyddot;
252 m_yd = _data->
yd - _data->
yddot*timestep;
255 m_nextyd = _data->
yd;
256 m_nextyddot = _data->
yddot;
258 m_yddot = (_data->
yd-m_yd)/timestep;
261 m_yddot = m_nextyddot;
280 *(
double*)&m_data.
y =
m_chi(2);
283 m_data.
yddot = m_yddot;
293 bool cacheAvail =
true;
300 *(
double*)&m_data.
y =
m_chi(2);
303 m_data.
yddot = m_yddot;
313 m_yddot = m_nextyddot;
typedef double(DMatrix)[4][4]
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers....
int JntToJac(const JntArray &q_in, Jacobian &jac)
void addSegment(const Segment &segment)
represents a frame transformation in 3D space (rotation + translation)
Rotation M
Orientation of the Frame.
Vector p
origine of the Frame
Frame Inverse() const
Gives back inverse transformation of a Frame.
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
represents rotations in 3 dimensional space.
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and an inertia) with...
int addChannel(const void *device, const char *name, unsigned int maxItemSize)
void * addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length)
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
KDL::Frame m_internalPose
KDL::Frame m_externalPose
ConstraintCallback m_constraintCallback
virtual void initCache(Cache *_cache)
virtual void updateKinematics(const Timestamp ×tamp)
Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100)
virtual bool setControlParameters(struct ConstraintValues *_values, unsigned int _nvalues, double timestep)
virtual void updateControlOutput(const Timestamp ×tamp)
virtual void updateJacobian()
virtual void pushCache(const Timestamp ×tamp)
virtual bool initialise(Frame &init_pose)
virtual const ConstraintValues * getControlParameters(unsigned int *_nvalues)
ccl_device_inline float2 fabs(const float2 &a)
static void error(const char *str)
INLINE S Norm(const Rall1d< T, V, S > &value)
const double PI
the value of pi
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
static const unsigned int distanceCacheSize
struct ConstraintSingleValue * values
ccl_device_inline float beta(float x, float y)