forked from bartvdbraak/blender
Fix for commit 41227 (Some opening comment tags (/*) were lost!).
This commit is contained in:
parent
82c84f4b58
commit
8a6a3dbb54
@ -80,7 +80,7 @@ void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp
|
||||
|
||||
double ConstraintSet::getMaxTimestep(double& timestep)
|
||||
{
|
||||
e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
|
||||
e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff();
|
||||
if (timestep*maxChidot > m_maxDeltaChi) {
|
||||
timestep = m_maxDeltaChi/maxChidot;
|
||||
}
|
||||
@ -162,9 +162,9 @@ bool ConstraintSet::closeLoop(){
|
||||
}else
|
||||
m_B.row(i) = m_U.col(i)/m_S(i);
|
||||
|
||||
m_Jf_inv.noalias()=m_V*m_B;
|
||||
m_Jf_inv=(m_V*m_B).lazy();
|
||||
|
||||
m_chi.noalias()+=m_Jf_inv*m_tdelta;
|
||||
m_chi+=(m_Jf_inv*m_tdelta).lazy();
|
||||
updateJacobian();
|
||||
// m_externalPose-m_internalPose in end effector frame
|
||||
// this is just to compare the pose, a different formula would work too
|
||||
|
@ -54,7 +54,7 @@ const e_matrix& ControlledObject::getJq(unsigned int ee) const
|
||||
|
||||
double ControlledObject::getMaxTimestep(double& timestep)
|
||||
{
|
||||
e_scalar maxQdot = m_qdot.array().abs().maxCoeff();
|
||||
e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff();
|
||||
if (timestep*maxQdot > m_maxDeltaQ) {
|
||||
timestep = m_maxDeltaQ/maxQdot;
|
||||
}
|
||||
|
@ -473,7 +473,7 @@ double CopyPose::getMaxTimestep(double& timestep)
|
||||
// CopyPose should not have any limit on linear velocity:
|
||||
// in case the target is out of reach, this can be very high.
|
||||
// We will simply limit on rotation
|
||||
e_scalar maxChidot = m_chidot.block(3,0,3,1).array().abs().maxCoeff();
|
||||
e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff();
|
||||
if (timestep*maxChidot > m_maxDeltaChi) {
|
||||
timestep = m_maxDeltaChi/maxChidot;
|
||||
}
|
||||
|
@ -356,7 +356,7 @@ bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, b
|
||||
m_Uf.col(i).setConstant(0.0);
|
||||
else
|
||||
m_Uf.col(i)*=(1/m_Sf(i));
|
||||
project(m_Jf_inv,cs->featurerange,cs->featurerange).noalias()=m_Vf*m_Uf.transpose();
|
||||
project(m_Jf_inv,cs->featurerange,cs->featurerange)=(m_Vf*m_Uf.transpose()).lazy();
|
||||
|
||||
//Get the robotjacobian associated with this constraintset
|
||||
//Each jacobian is expressed in robot base frame => convert to world reference
|
||||
@ -410,11 +410,11 @@ bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, b
|
||||
}
|
||||
|
||||
//Calculate A
|
||||
m_Atemp.noalias()=m_Cf*m_Jf_inv;
|
||||
m_A.noalias() = m_Cq-(m_Atemp*m_Jq);
|
||||
m_Atemp=(m_Cf*m_Jf_inv).lazy();
|
||||
m_A = m_Cq-(m_Atemp*m_Jq).lazy();
|
||||
if (m_nuTotal > 0) {
|
||||
m_B.noalias()=m_Atemp*m_Ju;
|
||||
m_ydot.noalias() += m_B*m_xdot;
|
||||
m_B=(m_Atemp*m_Ju).lazy();
|
||||
m_ydot += (m_B*m_xdot).lazy();
|
||||
}
|
||||
|
||||
//Call the solver with A, Wq, Wy, ydot to solver qdot:
|
||||
@ -435,13 +435,13 @@ bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, b
|
||||
//Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
|
||||
e_vector6 external_vel = e_zero_vector(6);
|
||||
if (ob1->jointrange.count > 0)
|
||||
external_vel.noalias() += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange));
|
||||
external_vel += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy();
|
||||
if (ob2->jointrange.count > 0)
|
||||
external_vel.noalias() += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange));
|
||||
external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy();
|
||||
if (ob1->coordinaterange.count > 0)
|
||||
external_vel.noalias() += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange));
|
||||
external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy();
|
||||
if (ob2->coordinaterange.count > 0)
|
||||
external_vel.noalias() += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange));
|
||||
external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy();
|
||||
//the twist caused by the constraint must be opposite because of the closed loop
|
||||
//estimate the velocity of the joints using the inverse jacobian
|
||||
e_vector6 estimated_chidot = project(m_Jf_inv,cs->featurerange,cs->featurerange)*(-external_vel);
|
||||
|
@ -65,10 +65,10 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd
|
||||
if(ret<0)
|
||||
return false;
|
||||
|
||||
m_WqV.noalias() = Wq*m_V;
|
||||
m_WqV = (Wq*m_V).lazy();
|
||||
|
||||
//Wy*ydot
|
||||
m_Wy_ydot = Wy.array() * ydot.array();
|
||||
m_Wy_ydot = Wy.cwise() * ydot;
|
||||
//S^-1*U'*Wy*ydot
|
||||
e_scalar maxDeltaS = e_scalar(0.0);
|
||||
e_scalar prevS = e_scalar(0.0);
|
||||
@ -85,7 +85,7 @@ bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& yd
|
||||
}
|
||||
lambda = (S < m_epsilon) ? (e_scalar(1.0)-KDL::sqr(S/m_epsilon))*m_lambda*m_lambda : e_scalar(0.0);
|
||||
alpha = m_U.col(i).dot(m_Wy_ydot)*S/(S*S+lambda);
|
||||
vmax = m_WqV.col(i).array().abs().maxCoeff();
|
||||
vmax = m_WqV.col(i).cwise().abs().maxCoeff();
|
||||
norm = fabs(alpha*vmax);
|
||||
if (norm > m_qmax) {
|
||||
qdot += m_WqV.col(i)*(alpha*m_qmax/norm);
|
||||
|
@ -60,7 +60,7 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
|
||||
e_scalar N, M;
|
||||
|
||||
// Create the Weighted jacobian
|
||||
m_AWq.noalias() = A*Wq;
|
||||
m_AWq = (A*Wq).lazy();
|
||||
for (i=0; i<m_nc; i++)
|
||||
m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
|
||||
|
||||
@ -75,8 +75,8 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
|
||||
if(ret<0)
|
||||
return false;
|
||||
|
||||
m_Wy_ydot = Wy.array() * ydot.array();
|
||||
m_WqV.noalias() = Wq*m_V;
|
||||
m_Wy_ydot = Wy.cwise() * ydot;
|
||||
m_WqV = (Wq*m_V).lazy();
|
||||
qdot.setZero();
|
||||
e_scalar maxDeltaS = e_scalar(0.0);
|
||||
e_scalar prevS = e_scalar(0.0);
|
||||
@ -121,7 +121,7 @@ bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& y
|
||||
M *= Sinv;
|
||||
alpha = m_U.col(i).dot(m_Wy_ydot);
|
||||
_qmax = (N < M) ? m_qmax*N/M : m_qmax;
|
||||
vmax = m_WqV.col(i).array().abs().maxCoeff();
|
||||
vmax = m_WqV.col(i).cwise().abs().maxCoeff();
|
||||
norm = fabs(Sinv*alpha*vmax);
|
||||
if (norm > _qmax) {
|
||||
damp = Sinv*alpha*_qmax/norm;
|
||||
|
@ -1,4 +1,4 @@
|
||||
*
|
||||
/*
|
||||
* ***** BEGIN GPL LICENSE BLOCK *****
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
|
Loading…
Reference in New Issue
Block a user