Add Chromium-only Blender WebEngine parity work
This commit is contained in:
mes123456
322
blender-5.2.0/intern/itasc/Distance.cpp
Normal file
322
blender-5.2.0/intern/itasc/Distance.cpp
Normal file
@@ -0,0 +1,322 @@
|
||||
/* SPDX-FileCopyrightText: 2009 Ruben Smits
|
||||
*
|
||||
* SPDX-License-Identifier: LGPL-2.1-or-later */
|
||||
|
||||
/** \file
|
||||
* \ingroup intern_itasc
|
||||
*/
|
||||
|
||||
#include "Distance.hpp"
|
||||
#include "kdl/kinfam_io.hpp"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
namespace iTaSC
|
||||
{
|
||||
// a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot
|
||||
static const unsigned int distanceCacheSize = sizeof(double)*5 + sizeof(e_scalar)*6;
|
||||
|
||||
Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations):
|
||||
ConstraintSet(1,accuracy,maximum_iterations),
|
||||
m_chiKdl(6),m_jac(6),m_cache(NULL),
|
||||
m_distCCh(-1),m_distCTs(0)
|
||||
{
|
||||
m_chain.addSegment(Segment(Joint(Joint::RotZ)));
|
||||
m_chain.addSegment(Segment(Joint(Joint::RotX)));
|
||||
m_chain.addSegment(Segment(Joint(Joint::TransY)));
|
||||
m_chain.addSegment(Segment(Joint(Joint::RotZ)));
|
||||
m_chain.addSegment(Segment(Joint(Joint::RotY)));
|
||||
m_chain.addSegment(Segment(Joint(Joint::RotX)));
|
||||
|
||||
m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
|
||||
m_jacsolver = new KDL::ChainJntToJacSolver(m_chain);
|
||||
m_Cf(0,2)=1.0;
|
||||
m_alpha = 1.0;
|
||||
m_tolerance = 0.05;
|
||||
m_maxerror = armlength/2.0;
|
||||
m_K = 20.0;
|
||||
m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
|
||||
m_yddot = m_nextyddot = 0.0;
|
||||
m_yd = m_nextyd = KDL::epsilon;
|
||||
memset(&m_data, 0, sizeof(m_data));
|
||||
// initialize the data with normally fixed values
|
||||
m_data.id = ID_DISTANCE;
|
||||
m_values.id = ID_DISTANCE;
|
||||
m_values.number = 1;
|
||||
m_values.alpha = m_alpha;
|
||||
m_values.feedback = m_K;
|
||||
m_values.tolerance = m_tolerance;
|
||||
m_values.values = &m_data;
|
||||
}
|
||||
|
||||
Distance::~Distance()
|
||||
{
|
||||
delete m_fksolver;
|
||||
delete m_jacsolver;
|
||||
}
|
||||
|
||||
bool Distance::computeChi(Frame& pose)
|
||||
{
|
||||
double dist, alpha, beta, gamma;
|
||||
dist = pose.p.Norm();
|
||||
Rotation basis;
|
||||
if (dist < KDL::epsilon) {
|
||||
// distance is almost 0, no need for initial rotation
|
||||
m_chi(0) = 0.0;
|
||||
m_chi(1) = 0.0;
|
||||
} else {
|
||||
// find the XZ angles that bring the Y axis to point to init_pose.p
|
||||
Vector axis(pose.p/dist);
|
||||
beta = 0.0;
|
||||
if (fabs(axis(2)) > 1-KDL::epsilon) {
|
||||
// direction is aligned on Z axis, just rotation on X
|
||||
alpha = 0.0;
|
||||
gamma = KDL::sign(axis(2))*KDL::PI/2;
|
||||
} else {
|
||||
alpha = -KDL::atan2(axis(0), axis(1));
|
||||
gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1))));
|
||||
}
|
||||
// rotation after first 2 joints
|
||||
basis = Rotation::EulerZYX(alpha, beta, gamma);
|
||||
m_chi(0) = alpha;
|
||||
m_chi(1) = gamma;
|
||||
}
|
||||
m_chi(2) = dist;
|
||||
basis = basis.Inverse()*pose.M;
|
||||
basis.GetEulerZYX(alpha, beta, gamma);
|
||||
// alpha = rotation on Z
|
||||
// beta = rotation on Y
|
||||
// gamma = rotation on X in that order
|
||||
// it corresponds to the joint order, so just assign
|
||||
m_chi(3) = alpha;
|
||||
m_chi(4) = beta;
|
||||
m_chi(5) = gamma;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Distance::initialise(Frame& init_pose)
|
||||
{
|
||||
// we will initialize m_chi to values that match the pose
|
||||
m_externalPose=init_pose;
|
||||
computeChi(m_externalPose);
|
||||
// get current Jf and update internal pose
|
||||
updateJacobian();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Distance::closeLoop()
|
||||
{
|
||||
if (!Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold)){
|
||||
computeChi(m_externalPose);
|
||||
updateJacobian();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Distance::initCache(Cache *_cache)
|
||||
{
|
||||
m_cache = _cache;
|
||||
m_distCCh = -1;
|
||||
if (m_cache) {
|
||||
// create one channel for the coordinates
|
||||
m_distCCh = m_cache->addChannel(this, "Xf", distanceCacheSize);
|
||||
// save initial constraint in cache position 0
|
||||
pushDist(0);
|
||||
}
|
||||
}
|
||||
|
||||
void Distance::pushDist(CacheTS timestamp)
|
||||
{
|
||||
if (m_distCCh >= 0) {
|
||||
double *item = (double*)m_cache->addCacheItem(this, m_distCCh, timestamp, NULL, distanceCacheSize);
|
||||
if (item) {
|
||||
*item++ = m_K;
|
||||
*item++ = m_tolerance;
|
||||
*item++ = m_yd;
|
||||
*item++ = m_yddot;
|
||||
*item++ = m_alpha;
|
||||
memcpy(item, &m_chi[0], 6*sizeof(e_scalar));
|
||||
}
|
||||
m_distCTs = timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
bool Distance::popDist(CacheTS timestamp)
|
||||
{
|
||||
if (m_distCCh >= 0) {
|
||||
double *item = (double*)m_cache->getPreviousCacheItem(this, m_distCCh, ×tamp);
|
||||
if (item && timestamp != m_distCTs) {
|
||||
m_values.feedback = m_K = *item++;
|
||||
m_values.tolerance = m_tolerance = *item++;
|
||||
m_yd = *item++;
|
||||
m_yddot = *item++;
|
||||
m_values.alpha = m_alpha = *item++;
|
||||
memcpy(&m_chi[0], item, 6*sizeof(e_scalar));
|
||||
m_distCTs = timestamp;
|
||||
m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
|
||||
updateJacobian();
|
||||
}
|
||||
return (item) ? true : false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Distance::pushCache(const Timestamp& timestamp)
|
||||
{
|
||||
if (!timestamp.substep && timestamp.cache)
|
||||
pushDist(timestamp.cacheTimestamp);
|
||||
}
|
||||
|
||||
void Distance::updateKinematics(const Timestamp& timestamp)
|
||||
{
|
||||
if (timestamp.interpolate) {
|
||||
//the internal pose and Jf is already up to date (see model_update)
|
||||
//update the desired output based on yddot
|
||||
if (timestamp.substep) {
|
||||
m_yd += m_yddot*timestamp.realTimestep;
|
||||
if (m_yd < KDL::epsilon)
|
||||
m_yd = KDL::epsilon;
|
||||
} else {
|
||||
m_yd = m_nextyd;
|
||||
m_yddot = m_nextyddot;
|
||||
}
|
||||
}
|
||||
pushCache(timestamp);
|
||||
}
|
||||
|
||||
void Distance::updateJacobian()
|
||||
{
|
||||
for(unsigned int i=0;i<6;i++)
|
||||
m_chiKdl[i]=m_chi[i];
|
||||
|
||||
m_fksolver->JntToCart(m_chiKdl,m_internalPose);
|
||||
m_jacsolver->JntToJac(m_chiKdl,m_jac);
|
||||
changeRefPoint(m_jac,-m_internalPose.p,m_jac);
|
||||
for(unsigned int i=0;i<6;i++)
|
||||
for(unsigned int j=0;j<6;j++)
|
||||
m_Jf(i,j)=m_jac(i,j);
|
||||
}
|
||||
|
||||
bool Distance::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
|
||||
{
|
||||
int action = 0;
|
||||
int i;
|
||||
ConstraintSingleValue* _data;
|
||||
|
||||
while (_nvalues > 0) {
|
||||
if (_values->id == ID_DISTANCE) {
|
||||
if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
|
||||
m_alpha = _values->alpha;
|
||||
action |= ACT_ALPHA;
|
||||
}
|
||||
if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
|
||||
m_tolerance = _values->tolerance;
|
||||
action |= ACT_TOLERANCE;
|
||||
}
|
||||
if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
|
||||
m_K = _values->feedback;
|
||||
action |= ACT_FEEDBACK;
|
||||
}
|
||||
for (_data = _values->values, i=0; i<_values->number; i++, _data++) {
|
||||
if (_data->id == ID_DISTANCE) {
|
||||
switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
|
||||
case 0:
|
||||
// no indication, keep current values
|
||||
break;
|
||||
case ACT_VELOCITY:
|
||||
// only the velocity is given estimate the new value by integration
|
||||
_data->yd = m_yd+_data->yddot*timestep;
|
||||
// walkthrough for negative value correction
|
||||
case ACT_VALUE:
|
||||
// only the value is given, estimate the velocity from previous value
|
||||
if (_data->yd < KDL::epsilon)
|
||||
_data->yd = KDL::epsilon;
|
||||
m_nextyd = _data->yd;
|
||||
// if the user sets the value, we assume future velocity is zero
|
||||
// (until the user changes the value again)
|
||||
m_nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
|
||||
if (timestep>0.0) {
|
||||
m_yddot = (_data->yd-m_yd)/timestep;
|
||||
} else {
|
||||
// allow the user to change target instantenously when this function
|
||||
// if called from setControlParameter with timestep = 0
|
||||
m_yddot = m_nextyddot;
|
||||
m_yd = m_nextyd;
|
||||
}
|
||||
break;
|
||||
case (ACT_VALUE|ACT_VELOCITY):
|
||||
// the user should not set the value and velocity at the same time.
|
||||
// In this case, we will assume that he want to set the future value
|
||||
// and we compute the current value to match the velocity
|
||||
if (_data->yd < KDL::epsilon)
|
||||
_data->yd = KDL::epsilon;
|
||||
m_yd = _data->yd - _data->yddot*timestep;
|
||||
if (m_yd < KDL::epsilon)
|
||||
m_yd = KDL::epsilon;
|
||||
m_nextyd = _data->yd;
|
||||
m_nextyddot = _data->yddot;
|
||||
if (timestep>0.0) {
|
||||
m_yddot = (_data->yd-m_yd)/timestep;
|
||||
} else {
|
||||
m_yd = m_nextyd;
|
||||
m_yddot = m_nextyddot;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
_nvalues--;
|
||||
_values++;
|
||||
}
|
||||
if (action & (ACT_TOLERANCE|ACT_FEEDBACK|ACT_ALPHA)) {
|
||||
// recompute the weight
|
||||
m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
const ConstraintValues* Distance::getControlParameters(unsigned int* _nvalues)
|
||||
{
|
||||
*(double*)&m_data.y = m_chi(2);
|
||||
*(double*)&m_data.ydot = m_ydot(0);
|
||||
m_data.yd = m_yd;
|
||||
m_data.yddot = m_yddot;
|
||||
m_data.action = 0;
|
||||
m_values.action = 0;
|
||||
if (_nvalues)
|
||||
*_nvalues=1;
|
||||
return &m_values;
|
||||
}
|
||||
|
||||
void Distance::updateControlOutput(const Timestamp& timestamp)
|
||||
{
|
||||
bool cacheAvail = true;
|
||||
if (!timestamp.substep) {
|
||||
if (!timestamp.reiterate)
|
||||
cacheAvail = popDist(timestamp.cacheTimestamp);
|
||||
}
|
||||
if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
|
||||
// initialize first callback the application to get the current values
|
||||
*(double*)&m_data.y = m_chi(2);
|
||||
*(double*)&m_data.ydot = m_ydot(0);
|
||||
m_data.yd = m_yd;
|
||||
m_data.yddot = m_yddot;
|
||||
m_data.action = 0;
|
||||
m_values.action = 0;
|
||||
if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) {
|
||||
setControlParameters(&m_values, 1, timestamp.realTimestep);
|
||||
}
|
||||
}
|
||||
if (!cacheAvail || !timestamp.interpolate) {
|
||||
// first position in cache: set the desired output immediately as we cannot interpolate
|
||||
m_yd = m_nextyd;
|
||||
m_yddot = m_nextyddot;
|
||||
}
|
||||
double error = m_yd-m_chi(2);
|
||||
if (KDL::Norm(error) > m_maxerror)
|
||||
error = KDL::sign(error)*m_maxerror;
|
||||
m_ydot(0)=m_yddot+m_K*error;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user