void next_k(int inNumSamples)
{
float newFreq = in0(1);
float newQ = in0(2);
float newHPCutoff = in0(3);
if (q != newQ)
set_q(newQ);
if (newHPCutoff != hpCutoff)
setFeedbackHPF(newHPCutoff * sampleDur());
double a, a_inv, a2, b, b2, c, g, g0;
calcFilterCoefficients(newFreq, a, a2, a_inv, b, b2, c, g, g0);
double z0 = z[0];
double z1 = z[1];
double z2 = z[2];
double z3 = z[3];
double z4 = z[4];
const float * inSig = zin(0);
float * outSig = zout(0);
for (int i = 0; i != inNumSamples; ++i) {
double x = ZXP(inSig);
ZXP(outSig) = tick(x, a, a2, a_inv, b, b2, c, g, g0, z0, z1, z2, z3, z4);
}
z[0] = z0;
z[1] = z1;
z[2] = z2;
z[3] = z3;
z[4] = z4;
}
void CMultitaskLogisticRegression::initialize_parameters()
{
set_z(0.0);
set_q(2.0);
set_termination(0);
set_regularization(0);
set_tolerance(1e-3);
set_max_iter(1000);
}
void FreeJoint::updateTransform_Issue122(double _timeStep) {
mT_Joint = mT_Joint * math::expMap(_timeStep * get_dq());
set_q(math::logMap(mT_Joint));
mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}
scalar sasfit_sq_crec_aniso_pearson(scalar x, sasfit_param * param)
{
SASFIT_ASSERT_PTR(param); // assert pointer param is valid
SASFIT_CHECK_COND1((PAD < 0.0), param, "a(%lg) < 0",PAD); // modify condition to your needs
SASFIT_CHECK_COND1((PBD < 0.0), param, "a(%lg) < 0",PBD); // modify condition to your needs
SASFIT_CHECK_COND1((SIGMA_A < 0.0), param, "a(%lg) < 0",SIGMA_A); // modify condition to your needs
SASFIT_CHECK_COND1((PDELTAQ < 0.0), param, "a(%lg) < 0",PDELTAQ); // modify condition to your needs
SASFIT_CHECK_COND1((PDELTAPSI < 0.0), param, "a(%lg) < 0",PDELTAPSI); // modify condition to your needs
SASFIT_CHECK_COND1((PLAMBDA < 0.0), param, "a(%lg) < 0",PLAMBDA); // modify condition to your needs
SASFIT_CHECK_COND1((PMAXHKL < 0.0), param, "a(%lg) < 0",PMAXHKL); // modify condition to your needs
SASFIT_CHECK_COND1((PNU < 1.0), param, "a(%lg) < 1",PNU); // modify condition to your needs
SASFIT_CHECK_COND1((PNU > 100.0), param, "a(%lg) > 100",PNU); // modify condition to your needs
// insert your code here
PEAKSHAPE = PEARSON;
ORDERTYPE = CREC;
ADVAL = PAD;
BDVAL = PBD;
DELTAQVAL = PDELTAQ;
DELTAPSIVAL = PDELTAPSI;
LAMBDAVAL = PLAMBDA;
ALPHAVAL = PROLL*M_PI/180.;
BETAVAL = PPITCH*M_PI/180.;
GAMMAVAL = PYAW*M_PI/180.;
if (PSI_DEG >= 0) {
QMODVAL = x;
VARPHIVAL = sasfit_param_override_get_psi(PSI_DEG*M_PI/180.);
} else {
QMODVAL = -PSI_DEG;
VARPHIVAL = sasfit_param_override_get_psi(x*M_PI/180.);
}
// VARPHIVAL = sasfit_param_override_get_psi(PSI_DEG*M_PI/180.);
VARTHETAVAL = 2*asin(QMODVAL*LAMBDAVAL/(4*M_PI));
MAXHKLVAL = PMAXHKL;
NUVAL = PNU;
set_ki(&ospParameter,param);
set_ks(&ospParameter,param);
set_q(&ospParameter,param);
ops_setConvention(&ospParameter,DEFAULT_CONVENTION);
ops_setEulerAngles(&ospParameter,ALPHAVAL,BETAVAL,GAMMAVAL);
init_osp(&ospParameter,param);
return Lattice_Factor_aniso(&ospParameter,param)*G(&ospParameter,param)+(1.0-G(&ospParameter,param));
}
void SynthChannel::prepareToPlay(double sampleRate, int samplesPerBlock, int numOutputChannels)
{
filters.clear(true);
for (int i = 0; i < numOutputChannels; i++) {
auto filter = new DiodeLadderFilter();
filter->set_feedback_hpf_cutoff(FB_HPF_CUTOFF);
filter->set_fc((double)bus->getChannelParameterValue(ParameterID::Channel_Cutoff, this->channelNumber));
filter->set_q((double)bus->getChannelParameterValue(ParameterID::Channel_Resonance, this->channelNumber));
filters.add(filter);
}
this->sampleRate = sampleRate;
initializeIncrement();
initializeEnvelopeIncrement();
}
void Hamiltonian::set_q(boost::python::list q_) {
/**
Update Hamiltonian coordinates (all are real-valued scalars - the components of the Python list) - Python-friendly
\param[in] q The Python list of real-valued coordinates to be used for Hamiltonian calculations.
Note: this also sets the status_dia and status_adi variables to zero, impliying the Hamiltonian is not
up to date - which is what we want: since the coordinates are changed, the Hamiltonian must be recomputed
From the prafmatic point of view, if you call this function - expect slower performance.
*/
int sz = boost::python::len(q_);
vector<double> tmp_q(sz,0.0);
for(int i=0; i<sz; i++) {
tmp_q[i] = boost::python::extract<double>(q_[i]);
}
set_q(tmp_q);
}
void Robot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
const ColumnVector & dqp,
ColumnVector & ltorque, ColumnVector & dtorque)
{
int i;
ColumnVector z0(3);
Matrix Rt, temp;
Matrix Q(3,3);
ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
ColumnVector *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp;
if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
ltorque = ColumnVector(dof);
dtorque = ColumnVector(dof);
set_q(q);
w = new ColumnVector[dof+1];
wp = new ColumnVector[dof+1];
vp = new ColumnVector[dof+1];
a = new ColumnVector[dof+1];
f = new ColumnVector[dof+1];
n = new ColumnVector[dof+1];
F = new ColumnVector[dof+1];
N = new ColumnVector[dof+1];
p = new ColumnVector[dof+1];
dw = new ColumnVector[dof+1];
dwp = new ColumnVector[dof+1];
dvp = new ColumnVector[dof+1];
da = new ColumnVector[dof+1];
df = new ColumnVector[dof+1];
dn = new ColumnVector[dof+1];
dF = new ColumnVector[dof+1];
dN = new ColumnVector[dof+1];
dp = new ColumnVector[dof+1];
w[0] = ColumnVector(3);
wp[0] = ColumnVector(3);
vp[0] = gravity;
dw[0] = ColumnVector(3);
dwp[0] = ColumnVector(3);
dvp[0] = ColumnVector(3);
z0 = 0.0;
Q = 0.0;
Q(1,2) = -1.0;
Q(2,1) = 1.0;
z0(3) = 1.0;
w[0] = 0.0;
wp[0] = 0.0;
dw[0] = 0.0;
dwp[0] = 0.0;
dvp[0] = 0.0;
for(i = 1; i <= dof; i++) {
Rt = links[i].R.t();
p[i] = ColumnVector(3);
p[i](1) = links[i].get_a();
p[i](2) = links[i].get_d() * Rt(2,3);
p[i](3) = links[i].get_d() * Rt(3,3);
if(links[i].get_joint_type() != 0) {
dp[i] = ColumnVector(3);
dp[i](1) = 0.0;
dp[i](2) = Rt(2,3);
dp[i](3) = Rt(3,3);
}
if(links[i].get_joint_type() == 0) {
w[i] = Rt*(w[i-1] + z0*qp(i));
dw[i] = Rt*(dw[i-1] + z0*dqp(i));
wp[i] = Rt*(wp[i-1] + vec_x_prod(w[i-1],z0*qp(i)));
dwp[i] = Rt*(dwp[i-1]
+ vec_x_prod(dw[i-1],z0*qp(i))
+ vec_x_prod(w[i-1],z0*dqp(i))
);
vp[i] = vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
+ Rt*(vp[i-1]);
dvp[i] = vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
+ Rt*dvp[i-1];
} else {
w[i] = Rt*w[i-1];
dw[i] = Rt*dw[i-1];
wp[i] = Rt*wp[i-1];
dwp[i] = Rt*dwp[i-1];
vp[i] = Rt*(vp[i-1]
+ vec_x_prod(w[i],z0*qp(i))) * 2.0
+ vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
dvp[i] = Rt*(dvp[i-1]
+ (vec_x_prod(dw[i],z0*qp(i)) * 2.0
+ vec_x_prod(w[i],z0*dqp(i))))
+ vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]));
}
a[i] = vec_x_prod(wp[i],links[i].r)
+ vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
+ vp[i];
da[i] = vec_x_prod(dwp[i],links[i].r)
+ vec_x_prod(dw[i],vec_x_prod(w[i],links[i].r))
+ vec_x_prod(w[i],vec_x_prod(dw[i],links[i].r))
+ dvp[i];
}
for(i = dof; i >= 1; i--) {
F[i] = a[i] * links[i].m;
N[i] = links[i].I*wp[i] + vec_x_prod(w[i],links[i].I*w[i]);
dF[i] = da[i] * links[i].m;
dN[i] = links[i].I*dwp[i] + vec_x_prod(dw[i],links[i].I*w[i])
+ vec_x_prod(w[i],links[i].I*dw[i]);
if(i == dof) {
f[i] = F[i];
n[i] = vec_x_prod(p[i],f[i])
+ vec_x_prod(links[i].r,F[i]) + N[i];
df[i] = dF[i];
dn[i] = vec_x_prod(p[i],df[i])
+ vec_x_prod(links[i].r,dF[i]) + dN[i];
} else {
f[i] = links[i+1].R*f[i+1] + F[i];
n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
+ vec_x_prod(links[i].r,F[i]) + N[i];
df[i] = links[i+1].R*df[i+1] + dF[i];
dn[i] = links[i+1].R*dn[i+1] + vec_x_prod(p[i],df[i])
+ vec_x_prod(links[i].r,dF[i]) + dN[i];
}
if(links[i].get_joint_type() == 0) {
temp = ((z0.t()*links[i].R)*n[i]);
ltorque(i) = temp(1,1);
temp = ((z0.t()*links[i].R)*dn[i]);
dtorque(i) = temp(1,1);
} else {
temp = ((z0.t()*links[i].R)*f[i]);
ltorque(i) = temp(1,1);
temp = ((z0.t()*links[i].R)*df[i]);
dtorque(i) = temp(1,1);
}
}
delete []dp;
delete []dN;
delete []dF;
delete []dn;
delete []df;
delete []da;
delete []dvp;
delete []dwp;
delete []dw;
delete []p;
delete []N;
delete []F;
delete []n;
delete []f;
delete []a;
delete []vp;
delete []wp;
delete []w;
}
