tmaq
/
tornavis
1
0
Fork 0
Code Issues 2 Pull Requests 29 Packages Projects 24 Releases Wiki Activity

Another set of UI messages fixes and tweaks! No functional changes.

This commit is contained in:
Bastien Montagne 2011-10-23 19:54:06 +00:00
parent 8a6a3dbb54
commit c49cdf5eec
8 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
GNUmakefile
View File

@ -1,6 +1,5 @@
# -*- mode: gnumakefile; tab-width: 8; indent-tabs-mode: t; -*-
# vim: tabstop=8
# $Id: GNUmakefile 41122 2011-10-19 21:55:27Z campbellbarton $
# vim: tabstop=4
#
# ##### BEGIN GPL LICENSE BLOCK #####
#

4
SConstruct
View File

@ -1,5 +1,5 @@
#!/usr/bin/env python
# $Id: SConstruct 41169 2011-10-21 04:23:26Z campbellbarton $
#
# ***** BEGIN GPL LICENSE BLOCK *****
#
# This program is free software; you can redistribute it and/or
@ -30,6 +30,8 @@
# Then read all SConscripts and build
#
# TODO: fix /FORCE:MULTIPLE on windows to get proper debug builds.
# TODO: directory copy functions are far too complicated, see:
# http://wiki.blender.org/index.php/User:Ideasman42/SConsNotSimpleInstallingFiles
import platform as pltfrm

6
intern/itasc/ConstraintSet.cpp
View File

@ -80,7 +80,7 @@ void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp
double ConstraintSet::getMaxTimestep(double& timestep)
{
e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff();
e_scalar maxChidot = m_chidot.array().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=(m_V*m_B).lazy();
m_Jf_inv.noalias()=m_V*m_B;
m_chi+=(m_Jf_inv*m_tdelta).lazy();
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

2
intern/itasc/ControlledObject.cpp
View File

@ -54,7 +54,7 @@ const e_matrix& ControlledObject::getJq(unsigned int ee) const
double ControlledObject::getMaxTimestep(double& timestep)
{
e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff();
e_scalar maxQdot = m_qdot.array().abs().maxCoeff();
if (timestep*maxQdot > m_maxDeltaQ) {
timestep = m_maxDeltaQ/maxQdot;
}

2
intern/itasc/CopyPose.cpp
View File

@ -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).cwise().abs().maxCoeff();
e_scalar maxChidot = m_chidot.block(3,0,3,1).array().abs().maxCoeff();
if (timestep*maxChidot > m_maxDeltaChi) {
timestep = m_maxDeltaChi/maxChidot;
}

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)=(m_Vf*m_Uf.transpose()).lazy();
project(m_Jf_inv,cs->featurerange,cs->featurerange).noalias()=m_Vf*m_Uf.transpose();
//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=(m_Cf*m_Jf_inv).lazy();
m_A = m_Cq-(m_Atemp*m_Jq).lazy();
m_Atemp.noalias()=m_Cf*m_Jf_inv;
m_A.noalias() = m_Cq-(m_Atemp*m_Jq);
if (m_nuTotal > 0) {
m_B=(m_Atemp*m_Ju).lazy();
m_ydot += (m_B*m_xdot).lazy();
m_B.noalias()=m_Atemp*m_Ju;
m_ydot.noalias() += m_B*m_xdot;
}
//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 += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy();
external_vel.noalias() += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange));
if (ob2->jointrange.count > 0)
external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy();
external_vel.noalias() += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange));
if (ob1->coordinaterange.count > 0)
external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy();
external_vel.noalias() += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange));
if (ob2->coordinaterange.count > 0)
external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy();
external_vel.noalias() += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange));
//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);

6
intern/itasc/WDLSSolver.cpp
View File

@ -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 = (Wq*m_V).lazy();
m_WqV.noalias() = Wq*m_V;
//Wy*ydot
m_Wy_ydot = Wy.cwise() * ydot;
m_Wy_ydot = Wy.array() * ydot.array();
//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).cwise().abs().maxCoeff();
vmax = m_WqV.col(i).array().abs().maxCoeff();
norm = fabs(alpha*vmax);
if (norm > m_qmax) {
qdot += m_WqV.col(i)*(alpha*m_qmax/norm);

8
intern/itasc/WSDLSSolver.cpp
View File

@ -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 = (A*Wq).lazy();
m_AWq.noalias() = A*Wq;
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.cwise() * ydot;
m_WqV = (Wq*m_V).lazy();
m_Wy_ydot = Wy.array() * ydot.array();
m_WqV.noalias() = Wq*m_V;
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).cwise().abs().maxCoeff();
vmax = m_WqV.col(i).array().abs().maxCoeff();
norm = fabs(Sinv*alpha*vmax);
if (norm > _qmax) {
damp = Sinv*alpha*_qmax/norm;