archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity

Fix for commit 41227 (Some opening comment tags (/*) were lost!).

Browse Source
This commit is contained in:
Bastien Montagne
2011-10-23 19:39:20 +00:00
parent 82c84f4b58
commit 8a6a3dbb54
7 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
intern/itasc/Scene.cpp
View File

@@ -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);