Add Chromium-only Blender WebEngine parity work

blender-5.2.0/intern/itasc/ConstraintSet.cpp

@@ -0,0 +1,174 @@
+/* SPDX-FileCopyrightText: 2009 Ruben Smits
+ *
+ * SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/** \file
+ * \ingroup intern_itasc
+ */
+
+#include "ConstraintSet.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+
+namespace iTaSC {
+
+ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations):
+    m_nc(_nc),
+    m_Cf(e_zero_matrix(m_nc,6)),
+    m_Wy(e_scalar_vector(m_nc,1.0)),
+    m_y(m_nc),m_ydot(e_zero_vector(m_nc)),m_chi(e_zero_vector(6)),
+    m_S(6),m_temp(6),m_tdelta(6),
+    m_Jf(e_identity_matrix(6,6)),
+    m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)),
+    m_Jf_inv(e_zero_matrix(6,6)),
+	m_internalPose(F_identity), m_externalPose(F_identity),
+	m_constraintCallback(NULL), m_constraintParam(NULL), 
+	m_toggle(false),m_substep(false),
+    m_threshold(accuracy),m_maxIter(maximum_iterations)
+{
+	m_maxDeltaChi = e_scalar(0.52);
+}
+
+ConstraintSet::ConstraintSet():
+    m_nc(0), 
+	m_internalPose(F_identity), m_externalPose(F_identity),
+	m_constraintCallback(NULL), m_constraintParam(NULL), 
+	m_toggle(false),m_substep(false),
+	m_threshold(0.0),m_maxIter(0)
+{
+	m_maxDeltaChi = e_scalar(0.52);
+}
+
+void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)
+{
+    m_nc = _nc;
+    m_Jf = e_identity_matrix(6,6);
+    m_Cf = e_zero_matrix(m_nc,6);
+    m_U = e_identity_matrix(6,6);
+	m_V = e_identity_matrix(6,6);
+	m_B = e_zero_matrix(6,6);
+    m_Jf_inv = e_zero_matrix(6,6),
+    m_Wy = e_scalar_vector(m_nc,1.0),
+    m_chi = e_zero_vector(6);
+    m_chidot = e_zero_vector(6);
+	m_y = e_zero_vector(m_nc);
+	m_ydot = e_zero_vector(m_nc);
+	m_S = e_zero_vector(6);
+	m_temp = e_zero_vector(6);
+	m_tdelta = e_zero_vector(6);
+    m_threshold = accuracy;
+	m_maxIter = maximum_iterations;
+}
+
+ConstraintSet::~ConstraintSet() {
+
+}
+
+void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
+{
+    m_chi+=m_chidot*timestamp.realTimestep;
+	m_externalPose = _external_pose;
+
+    //update the internal pose and Jf
+    updateJacobian();
+	//check if loop is already closed, if not update the pose and Jf
+    unsigned int iter=0;
+    while(iter<5&&!closeLoop())
+        iter++;
+}
+
+double ConstraintSet::getMaxTimestep(double& timestep)
+{
+	e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
+	if (timestep*maxChidot > m_maxDeltaChi) {
+		timestep = m_maxDeltaChi/maxChidot;
+	}
+	return timestep;
+}
+
+bool ConstraintSet::initialise(Frame& init_pose){
+    m_externalPose=init_pose;
+	// get current Jf
+    updateJacobian();
+
+    unsigned int iter=0;
+    while(iter<m_maxIter&&!closeLoop()){
+        iter++;
+    }
+    if (iter<m_maxIter)
+        return true;
+    else
+        return false;
+}
+
+bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
+{
+	ConstraintValues values;
+	ConstraintSingleValue value;
+	values.values = &value;
+	values.number = 0;
+	values.action = action;
+	values.id = id;
+	value.action = action;
+	value.id = id;
+	switch (action) {
+	case ACT_NONE:
+		return true;
+	case ACT_VALUE:
+		value.yd = data;
+		values.number = 1;
+		break;
+	case ACT_VELOCITY:
+		value.yddot = data;
+		values.number = 1;
+		break;
+	case ACT_TOLERANCE:
+		values.tolerance = data;
+		break;
+	case ACT_FEEDBACK:
+		values.feedback = data;
+		break;
+	case ACT_ALPHA:
+		values.alpha = data;
+		break;
+	default:
+		assert(action==ACT_NONE);
+		break;
+	}
+	return setControlParameters(&values, 1, timestep);
+}
+
+bool ConstraintSet::closeLoop(){
+    //Invert Jf
+    //TODO: svd_boost_Macie has problems if Jf contains zero-rows
+    //toggle=!toggle;
+    //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
+	int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp);
+	if(ret<0)
+		return false;
+
+	// the reference point and frame of the jacobian is the base frame
+	// m_externalPose-m_internalPose is the twist to extend the end effector
+	// to get the required pose => change the reference point to the base frame
+	Twist twist_delta(diff(m_internalPose,m_externalPose));
+	twist_delta=twist_delta.RefPoint(-m_internalPose.p);
+	for(unsigned int i=0;i<6;i++)
+		m_tdelta(i)=twist_delta(i);
+	//TODO: use damping in constraintset inversion?
+	for(unsigned int i=0;i<6;i++) {
+		if(m_S(i)<m_threshold){
+			m_B.row(i).setConstant(0.0);
+		} else {
+			m_B.row(i) = m_U.col(i)/m_S(i);
+		}
+	}
+
+	m_Jf_inv.noalias()=m_V*m_B;
+
+	m_chi.noalias()+=m_Jf_inv*m_tdelta;
+	updateJacobian();
+	// m_externalPose-m_internalPose in end effector frame
+	// this is just to compare the pose, a different formula would work too
+	return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);
+
+}
+}