bool sol() {
for(int i=0; i<n-1; i++)
for(int j=0; j<k; j++)
Relax(e[j].u, e[j].v, e[j].r, e[j].c);
for(int i=0; i<k; i++)
if(Relax(e[i].u, e[i].v, e[i].r, e[i].c))
return true;
return false;
}
inline std::pair<size_t, size_t> LocalSimilarity(const Sequence& s1, const Sequence& s2) {
size_t m = s1.size();
size_t n = s2.size();
std::vector<std::vector<int>> a(m + 1);
for (size_t i = 0; i <= m; ++i) {
a[i].resize(n + 1);
}
for (size_t i = 0; i <= m; ++i) {
for (size_t j = 0; j <= n; ++j) {
a[i][j] = 0;
}
}
for (size_t i = 1; i <= m; ++i) {
for (size_t j = 1; j <= n; ++j) {
Relax(a[i][j], a[i - 1][j] - 1);
Relax(a[i][j], a[i][j - 1] - 1);
if (s1[i - 1] == s2[j - 1]) {
Relax(a[i][j], a[i - 1][j - 1] + 1);
} else {
Relax(a[i][j], a[i - 1][j - 1] - 1);
}
}
}
//finding local alignment
int answer = 0;
size_t i_m = 0;
size_t j_m = 0;
for (size_t i = 0; i <= m; ++i) {
for (size_t j = 0; j <= n; ++j) {
if (Relax(answer, a[i][j])) {
i_m = i;
j_m = j;
}
}
}
//finding alignment lengths
size_t i = i_m;
size_t j = j_m;
while (a[i][j] > 0) {
if (a[i][j] == a[i][j - 1] - 1) {
j--;
} else if (a[i][j] == a[i-1][j] - 1) {
i--;
} else if (a[i][j] == a[i-1][j-1] + 1) {
VERIFY(s1[i-1] == s2[j-1]);
i--;
j--;
} else {
VERIFY(a[i-1][j-1] - 1 == a[i][j] && s1[i-1] != s2[j-1]);
i--;
j--;
}
}
return std::make_pair(size_t(answer), std::min(i_m - i, j_m - j));
}
void Dijkstra()
{
long uNode,vNode,weight,ind;
InitializeSingleSource();
scanf("%ld",&source);
dist[source]=0;
heap_size=0;
memset(flag,0,sizeof(0));
Insert(source,0);
while(heap_size>0 && flag[0]<=vertex)
{
uNode=ExtractMin();
if(flag[uNode])
continue;
flag[0]+=1;
flag[uNode]=1;
for(ind=1; ind<=adj[uNode]; ind++)
{
vNode=adjMat[uNode][ind];
if(flag[vNode])
continue;
weight=costMat[uNode][vNode];
Relax(uNode,vNode,weight);
}
}
}
void AMWMot::OnCorruptedStart()
{
Relax();
// Shake the sensor in a crazy way
Sensor->GoCrazy(true);
bCrazy = true;
}
static void DepthFirstVisit( FuncWeight func_weight, std::shared_ptr< NodeType > node_src ){
int id_current_node = node_src->_id;
for( auto & adjacent : node_src->_desc ){
adjacent->_node_colour = graph_node_generic_colour::GREY;
if( Relax( node_src, adjacent, func_weight ) ){
DepthFirstVisit( func_weight, adjacent ); //if relaxed, descendents of adjacent node needs to be updated
}
}
}
//===================================================================================================
// PutArmToSleep
//===================================================================================================
void PutArmToSleep(void) {
g_fArmActive = false;
MoveArmTo(512, 212, 212, 512, 512, 256, 1000, true);
// And Relax all of the servos...
for(uint8_t i=1; i <= CNT_SERVOS; i++) {
Relax(i);
}
g_fServosFree = true;
}
/* (Called recursively.) */
void Umv(float *soln, float *dx, float *dy,
int w, int h, int numit, int mindim)
{
float *dx2=NULL, *dy2=NULL, *soln2=NULL;
int w2 = w/2, h2 = h/2;
if (!Coarsest(w, h, mindim)) {
Relax(soln, dx, dy, NULL, NULL, w, h, numit);
dx2 = Allocate(w2, h2, dx_type);
dy2 = Allocate(w2, h2, dy_type);
soln2 = Allocate(w2, h2, soln_type);
Restrict(dx2, dy2, w2, h2, dx, dy, NULL, NULL, soln, w, h);
Zero(soln2, w2, h2);
Umv(soln2, dx2, dy2, w2, h2, numit, mindim);
ProlongAndAccumulate(soln, w, h, soln2, w2, h2, NULL, NULL);
Relax(soln, dx, dy, NULL, NULL, w, h, numit);
}
else { /* coarsest */
Relax(soln, dx, dy, NULL, NULL, w, h, 2*w*h);
}
}
void Compute() {
mDists = *mGraph;
int n = mGraph->NumVertex();
for (int k = 0; k < n; ++k) {
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
Relax(i, j, k);
}
}
}
}
void AStar::Execute(const Graph &Graph, const string &VetexId)
{
const auto& Vertexes = Graph.GetVertexes();
Vertex* pVertexStart = Vertexes.find(VetexId)->second;
vector< Vertex* > Q;
// 初始化顶点
for (auto& it : Vertexes)
{
Vertex* pV = it.second;
pV->PathfindingData.Cost = 0;
pV->PathfindingData.pParent = nullptr;
pV->PathfindingData.Heuristic = 0x0FFFFFFF;
pV->PathfindingData.Flag = false;
}
// 初始化起始顶点
pVertexStart->PathfindingData.pParent = 0;
pVertexStart->PathfindingData.Cost = 0;
pVertexStart->PathfindingData.Heuristic = Estimate(pVertexStart, m_pVTarget);
// 把起始顶点放入列表中
Q.push_back(pVertexStart);
pVertexStart->PathfindingData.Flag = true;
for (; Q.size() > 0;)
{
// 选出最小路径估计的顶点
auto v = ExtractMin(Q);
v->PathfindingData.Flag = false;
if (v == m_pVTarget)
{
return;
}
// 对所有的出边进行“松弛”
const auto& EO = v->GetEdgesOut();
for (auto& it : EO)
{
Edge* pEdge = it.second;
Vertex* pVEnd = pEdge->GetEndVertex();
bool bRet = Relax(v, pVEnd, pEdge->GetWeight());
// 如果松弛成功，加入列表中
if (bRet && pVEnd->PathfindingData.Flag == false)
{
Q.push_back(pVEnd);
pVEnd->PathfindingData.Flag = true;
}
}
}
}
void Compute() {
DistanceType initDist = GetInitDistance();
for (int i = 0; i < mGraph->NumVertex(); ++i) {
mDistances[i] = initDist;
}
mDistances[mSource] = GetSourceDistance();
for (int i = 1; i < mGraph->NumVertex(); ++i) {
bool relaxed = false;
for (int j = 0; j < mGraph->NumEdge(); ++j) {
if (Relax(mGraph->GetEdge(j))) {
relaxed = true;
}
}
if (!relaxed) break;
}
mExistNegativeCycle = false;
for (int j = 0; j < mGraph->NumEdge(); ++j) {
if (Relax(mGraph->GetEdge(j))) {
mExistNegativeCycle = true;
break;
}
}
}
void vtkPolyDataSingleSourceShortestPath::ShortestPath(int startv, int endv)
{
int i, u, v;
InitSingleSource(startv);
HeapInsert(startv);
this->f->SetValue(startv, 1);
int stop = 0;
while ((u = HeapExtractMin()) >= 0 && !stop)
{
// u is now in s since the shortest path to u is determined
this->s->SetValue(u, 1);
// remove u from the front set
this->f->SetValue(u, 0);
if (u == endv && StopWhenEndReached)
stop = 1;
// Update all vertices v adjacent to u
for (i = 0; i < Adj[u]->GetNumberOfIds(); i++)
{
v = Adj[u]->GetId(i);
// s is the set of vertices with determined shortest path...do not use them again
if (!this->s->GetValue(v))
{
// Only relax edges where the end is not in s and edge is in the front set
double w = EdgeCost(this->GetInput(), u, v);
if (this->f->GetValue(v))
{
Relax(u, v, w);
}
// add edge v to front set
else
{
this->f->SetValue(v, 1);
this->d->SetValue(v, this->d->GetValue(u) + w);
// Set Predecessor of v to be u
this->pre->SetValue(v, u);
HeapInsert(v);
}
}
}
}
}
//--------------------------------------------------------------------
//[FREE SERVOS] Frees all the servos
//--------------------------------------------------------------------
void ServoDriver::FreeServos(void)
{
if (ServosEnabled) {
g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess...
SetRegOnAllServos(AX_TORQUE_ENABLE, 0);
#if 0
for (byte i = 0; i < NUMSERVOS; i++) {
Relax(pgm_read_byte(&cPinTable[i]));
}
#endif
g_InputController.AllowControllerInterrupts(true);
g_fServosFree = true;
}
}
// The main procedure of "Bellman-Ford" algorithm.
void Bellman_Ford()
{
InitialSingleSource();
for (int i = 0; i < ivc - 1; i++)
{
for (int curp = 1; curp <= ivc; curp++)
{
for (int cure = V[curp].first_edge; cure != 0; cure = E[cure].next)
{
Relax(curp, E[cure].endp, E[cure].w);
}
}
}
}
void Dijkstra(Grafo G, vertice* vertice_origem, int num_vertices){
int i = 0, j = 0, tam_heap = 0, posicao_vizinho = 0;
vertice *prox_fila;
vertice** heap;
// Inicializa o peso de todos os vértices até a origem com "infinito".
for(i = 1; i <= num_vertices; i++){
G[i]->peso_origem = 2147483646; /* Valor máximo possível para um inteiro. (Representa "infinito") */
G[i]->antecessor = NULL;
}
vertice_origem->peso_origem = 0;
heap = (vertice**) malloc((num_vertices+1)*sizeof(vertice*));
// Formar um vetor com o peso acumulado(desde a orgiem) de todos os vértices.
for(i = 1; i <= num_vertices; i++)
heap[i] = G[i];
tam_heap = num_vertices;
// Construir um heap, que deixará o o vértice com o menor peso na frente.
build_heap(heap, tam_heap);
j = 1;
while(tam_heap > 0){
// Verifica o vértice com o menor peso.
prox_fila = heap[j];
// Marca o vértice com uma flag, para indicar que o peso final do seu caminho mínimo já foi determinado.
prox_fila->flag = 1;
// Retira o vértice do heap.
tam_heap--;
// Verificar o peso da aresta para cada um dos vértices vizinhos.
vertice* proximo_vizinho;
proximo_vizinho = prox_fila->prox;
if(proximo_vizinho != NULL)
posicao_vizinho = proximo_vizinho->campo;
if(prox_fila->peso_origem == 2147483646)
break;
for(i = 0; i < prox_fila->num_vizinhos; i++){
// Se o peso final do do caminho até o vértice ainda não foi determinado, verifica se o peso do caminho mínimo pode ser "relaxado".
if(G[posicao_vizinho]->flag != 1)
Relax(G, prox_fila, G[posicao_vizinho]);
// Verifica qual vértice é o próximo vizinho a ser analisado.
proximo_vizinho = proximo_vizinho->prox;
if(proximo_vizinho != NULL)
posicao_vizinho = proximo_vizinho->campo;
}
// Reconstrói o heap.
build_heap(&(heap[j]), tam_heap);
j++;
}
free(heap);
}
void Dijkstra(struct graph *G,char s)
{
struct vertex *Q[G->count],*S[G->count],*u,*v;
struct edge *e;
int i=0;
n=G->count;
Initialize_Single_source(G,s);
for(v=G->head;v;v=v->vertexp,i++)
Q[i]=v;
i=0;
while(n!=0)
{
u=extract_min(Q);
S[i++]=u;
for(e=u->edgep;e;e=e->next)
Relax(u,e->destin,e->weight);
}
}
int Bellman_Ford(struct graph *G,int w,char s)
{
struct vertex *u;
struct edge *v;
int i;
Initialize_Single_source(G,s);
for(i=1;i<G->count;i++)
{
for(u=G->head;u;u=u->vertexp)
for(v=u->edgep;v;v=v->next)
Relax(u,v->destin,v->weight);
}
for(u=G->head;u;u=u->vertexp)
for(v=u->edgep;v;v=v->next)
if(v->destin->d > u->d+v->weight)
return 0;
return 1;
}
static void DijstraSearch( FuncWeight func_weight, std::shared_ptr< NodeType > node_src ){
std::set< std::shared_ptr< NodeType > > vertices_of_interest;
std::priority_queue< std::shared_ptr< NodeType >, std::vector< std::shared_ptr< NodeType > >, DijstraWeightCompare > min_queue;
node_src->_relaxed_weight = 0; //initialize source vertex
vertices_of_interest.insert( node_src );
min_queue.push( node_src );
while( !min_queue.empty() ){
auto current_node = min_queue.top(); //get the vertex with the lowest weight in min_queue
min_queue.pop();
for( auto & adjacent : current_node->_desc ){
Relax( current_node, adjacent, func_weight ); //relax path to adjacent vertex
if( vertices_of_interest.end() == vertices_of_interest.find( adjacent ) ){
min_queue.push( adjacent ); //add to min_queue and set if current adjacent vertex is not in there
vertices_of_interest.insert( adjacent );
}
}
}
}
void CWeaponShotEffector::Update()
{
if ( m_actived && m_cam_recoil.ReturnMode /*|| m_single_shot*/ )
{
Relax();
}
if ( !m_cam_recoil.ReturnMode && m_shot_end && !m_single_shot )
{
m_actived = false;
}
m_delta_vert = m_angle_vert - m_prev_angle_vert;
m_delta_horz = m_angle_horz - m_prev_angle_horz;
m_prev_angle_vert = m_angle_vert;
m_prev_angle_horz = m_angle_horz;
// Msg( " <<[%d] v=%.4f dv=%.4f a=%d s=%d fr=%d", m_shot_numer, m_angle_vert, m_delta_vert, m_actived, m_first_shot, Device.dwFrame );
}
static bool BreathFirstSearch( FuncWeight func_weight, std::shared_ptr< NodeType > node_src ){
std::queue< std::shared_ptr< NodeType > > queue_vertex;
node_src->_node_colour = graph_node_generic_colour::GREY;
node_src->_relaxed_weight = 0;
queue_vertex.push( node_src );
while( !queue_vertex.empty() ){
std::shared_ptr< NodeType > current_node( queue_vertex.front() );
queue_vertex.pop();
int id_current_node = current_node->_id;
for( auto & adjacent : current_node->_desc ){
// if( graph_node_generic_colour::WHITE == adjacent->_node_colour ){
adjacent->_node_colour = graph_node_generic_colour::GREY;
if( Relax( current_node, adjacent, func_weight ) ){
queue_vertex.push( adjacent ); //if relaxed, descendents of adjacent node needs to be updated
}
// }
}
// current_node->_node_colour = graph_node_generic_colour::BLACK;
}
return true;
}
// ======================================================================
bool TestVariableBlocking(const Epetra_Comm & Comm) {
// Basically each processor gets this 5x5 block lower-triangular matrix:
//
// [ 2 -1 0 0 0 ;...
// [-1 2 0 0 0 ;...
// [-1 -1 5 -1 -1 ;...
// [-1 -1 -1 5 -1 ;...
// [-1 -1 -1 -1 5 ];
//
// The nice thing about this matrix is that if the RHS is a constant,the solution is the same constant...
Epetra_Map RowMap(-1,5,0,Comm); // 5 rows per proc
Epetra_CrsMatrix A(Copy,RowMap,0);
int num_entries;
int indices[5];
double values[5];
int rb = RowMap.GID(0);
/*** Fill RHS / LHS ***/
Epetra_Vector rhs(RowMap), lhs(RowMap), exact_soln(RowMap);
rhs.PutScalar(2.0);
lhs.PutScalar(0.0);
exact_soln.PutScalar(2.0);
/*** Fill Matrix ****/
// Row 0
num_entries=2;
indices[0]=rb; indices[1]=rb+1;
values[0] =2; values[1] =-1;
A.InsertGlobalValues(rb,num_entries,&values[0],&indices[0]);
// Row 1
num_entries=2;
indices[0]=rb; indices[1]=rb+1;
values[0] =-1; values[1] =2;
A.InsertGlobalValues(rb+1,num_entries,&values[0],&indices[0]);
// Row 2
num_entries=5;
indices[0]=rb; indices[1]=rb+1; indices[2]=rb+2; indices[3]=rb+3; indices[4]=rb+4;
values[0] =-1; values[1] =-1; values[2] = 5; values[3] =-1; values[4] =-1;
A.InsertGlobalValues(rb+2,num_entries,&values[0],&indices[0]);
// Row 3
num_entries=5;
indices[0]=rb; indices[1]=rb+1; indices[2]=rb+2; indices[3]=rb+3; indices[4]=rb+4;
values[0] =-1; values[1] =-1; values[2] =-1; values[3] = 5; values[4] =-1;
A.InsertGlobalValues(rb+3,num_entries,&values[0],&indices[0]);
// Row 4
num_entries=5;
indices[0]=rb; indices[1]=rb+1; indices[2]=rb+2; indices[3]=rb+3; indices[4]=rb+4;
values[0] =-1; values[1] =-1; values[2] =-1; values[3] =-1; values[4] = 5;
A.InsertGlobalValues(rb+4,num_entries,&values[0],&indices[0]);
A.FillComplete();
/* Setup Block Relaxation */
int PartMap[5]={0,0,1,1,1};
Teuchos::ParameterList ilist;
ilist.set("partitioner: type","user");
ilist.set("partitioner: map",&PartMap[0]);
ilist.set("partitioner: local parts",2);
ilist.set("relaxation: sweeps",1);
ilist.set("relaxation: type","Gauss-Seidel");
Ifpack_BlockRelaxation<Ifpack_DenseContainer> Relax(&A);
Relax.SetParameters(ilist);
Relax.Initialize();
Relax.Compute();
Relax.ApplyInverse(rhs,lhs);
double norm;
lhs.Update(1.0,exact_soln,-1.0);
lhs.Norm2(&norm);
if(verbose) cout<<"Variable Block Partitioning Test"<<endl;
if(norm < 1e-14) {
if(verbose) cout << "Test passed" << endl;
return true;
}
else {
if(verbose) cout << "Test failed" << endl;
return false;
}
}
///////////////////////////////////////////////////////////////////////////////
// MAIN
///////////////////////////////////////////////////////////////////////////////
int main( int argc, char** argv ) {
// Command line inputs
bool show_gui = false;
bool use_ros = false;
walktype walk_type = walk_canned;
double walk_circle_radius = 5.0;
double walk_dist = 20;
double footsep_y = 0.085; // half of horizontal separation distance between feet
double foot_liftoff_z = 0.05; // foot liftoff height
double step_length = 0.15;
bool walk_sideways = false;
double com_height = 0.48; // height of COM above ANKLE
double com_ik_ascl = 0;
double zmpoff_y = 0; // lateral displacement between zmp and ankle
double zmpoff_x = 0;
double lookahead_time = 2.5;
double startup_time = 1.0;
double shutdown_time = 1.0;
double double_support_time = 0.05;
double single_support_time = 0.70;
size_t max_step_count = 4;
double zmp_jerk_penalty = 1e-8; // jerk penalty on ZMP controller
ik_error_sensitivity ik_sense = ik_strict;
// Parse command line inputs
const struct option long_options[] = {
{ "show-gui", no_argument, 0, 'g' },
{ "use-ros", no_argument, 0, 'R' },
{ "ik-errors", required_argument, 0, 'I' },
{ "walk-type", required_argument, 0, 'w' },
{ "walk-distance", required_argument, 0, 'D' },
{ "walk-circle-radius", required_argument, 0, 'r' },
{ "max-step-count", required_argument, 0, 'c' },
{ "foot-separation-y", required_argument, 0, 'y' },
{ "foot-liftoff-z", required_argument, 0, 'z' },
{ "step-length", required_argument, 0, 'l' },
{ "walk-sideways", no_argument, 0, 'S' },
{ "com-height", required_argument, 0, 'h' },
{ "comik-angle-weight", required_argument, 0, 'a' },
{ "zmp-offset-y", required_argument, 0, 'Y' },
{ "zmp-offset-x", required_argument, 0, 'X' },
{ "lookahead-time", required_argument, 0, 'T' },
{ "startup-time", required_argument, 0, 'p' },
{ "shutdown-time", required_argument, 0, 'n' },
{ "double-support-time", required_argument, 0, 'd' },
{ "single-support-time", required_argument, 0, 's' },
{ "zmp-jerk-penalty", required_argument, 0, 'P' },
{ "help", no_argument, 0, 'H' },
{ 0, 0, 0, 0 },
};
const char* short_options = "gRI:w:D:r:c:y:z:l:Sh:a:Y:X:T:p:n:d:s:P:H";
int opt, option_index;
while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 ) {
switch (opt) {
case 'g': show_gui = true; break;
case 'R': use_ros = true; break;
case 'I': ik_sense = getiksense(optarg); break;
case 'w': walk_type = getwalktype(optarg); break;
case 'D': walk_dist = getdouble(optarg); break;
case 'r': walk_circle_radius = getdouble(optarg); break;
case 'c': max_step_count = getlong(optarg); break;
case 'y': footsep_y = getdouble(optarg); break;
case 'z': foot_liftoff_z = getdouble(optarg); break;
case 'l': step_length = getdouble(optarg); break;
case 'S': walk_sideways = true; break;
case 'h': com_height = getdouble(optarg); break;
case 'a': com_ik_ascl = getdouble(optarg); break;
case 'Y': zmpoff_y = getdouble(optarg); break;
case 'X': zmpoff_x = getdouble(optarg); break;
case 'T': lookahead_time = getdouble(optarg); break;
case 'p': startup_time = getdouble(optarg); break;
case 'n': shutdown_time = getdouble(optarg); break;
case 'd': double_support_time = getdouble(optarg); break;
case 's': single_support_time = getdouble(optarg); break;
case 'P': zmp_jerk_penalty = getdouble(optarg); break;
case 'H': usage(std::cout); exit(0); break;
default: usage(std::cerr); exit(1); break;
}
}
/* Initialize ROS */
double frequency = 200;
//FIXME: ROS dependent
if(use_ros) {
ros::init( argc, argv, "publish_and_readonce" );
rosnode = new ros::NodeHandle();
loop_rate = new ros::Rate(frequency);
ros::Time last_ros_time_;
// Wait until sim is active (play)
bool wait = true;
while( wait ) {
last_ros_time_ = ros::Time::now();
if( last_ros_time_.toSec() > 0 ) {
wait = false;
}
}
// init ros joints
RosJointInit();
// ros topic subscribtions
ros::SubscribeOptions jointStatesSo =
ros::SubscribeOptions::create<sensor_msgs::JointState>(
"/atlas/joint_states", 1, GetJointStates,
ros::VoidPtr(), rosnode->getCallbackQueue());
jointStatesSo.transport_hints = ros::TransportHints().unreliable();
ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo);
pub_joint_commands_ =
rosnode->advertise<osrf_msgs::JointCommands>(
"/atlas/joint_commands", 1, true);
}
/* Initialize AK */
if(!_ak) {
DartLoader dart_loader;
World *mWorld = dart_loader.parseWorld(ATLAS_DATA_PATH "atlas/atlas_world.urdf");
_atlas = mWorld->getSkeleton("atlas");
_ak = new AtlasKinematics();
_ak->init(_atlas);
}
_atlas->setPose(_atlas->getPose().setZero(), true);
AtlasKinematics *AK = _ak;
/* Begin generating trajectories */
/* Trajectory that stores dof ticks */
vector<VectorXd> joint_traj;
/* Setup dofs initial conditions */
/* Relax */
VectorXd dofs = _atlas->getPose().setZero();
_atlas->setPose(dofs, true);
const int relax_ticks = 1000;
Relax(AK, _atlas, dofs, joint_traj, relax_ticks);
/* Walking variables */
IK_Mode mode[NUM_MANIPULATORS];
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
Vector3d comDelta = Vector3d::Zero();
Vector3d leftDelta = Vector3d::Zero();
Vector3d rightDelta = Vector3d::Zero();
int N = 0;
Matrix4d Twm[NUM_MANIPULATORS];
Twm[MANIP_L_FOOT] = AK->getLimbTransB(_atlas, MANIP_L_FOOT);
Twm[MANIP_R_FOOT] = AK->getLimbTransB(_atlas, MANIP_R_FOOT);
Matrix4d Twb;
Twb.setIdentity();
VectorXd dofs_save;
/* Move COM down */
comDelta << 0, 0, -0.04;
leftDelta.setZero();
rightDelta.setZero();
const int com_ticks = 1000;
genCOMIKTraj(AK, _atlas, Twb, Twm, dofs, comDelta, leftDelta, rightDelta, joint_traj, com_ticks);
/* ZMP Walking */
/* ZMP parameters */
// number of steps to walk
int numSteps = 12;
// lenght of a half step
double stepLength = 0.15;
// half foot seperation
dofs_save = _atlas->getPose();
cout << "********************************************" << endl;
cout << "Start ZMP walking" << endl;
cout << "*************************************" << endl;
cout << "POS ERROR: " << (dofs_save - dofs).norm() << endl;
cout << "*************************************" << endl;
_atlas->setPose(dofs);
double footSeparation = (AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(1,3) -
AK->getLimbTransW(_atlas, Twb, MANIP_R_FOOT)(1,3) ) / 2;
cout << "Half foot seperation: " << footSeparation << endl;
// one step time
double stepDuration = 1.0;
// move ZMP time
double slopeTime = 0.15;
// keep ZMP time
double levelTime = 0.85;
// command sending period
double dt = 1/frequency;
// height of COM
double zg = AK->getCOMW(_atlas, Twb)(2) - AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(2,3);
cout << "zg " << zg << endl;
// preview how many steps
int numPreviewSteps = 2;
// double Qe = 1;
// double R = 1e-6;
double Qe = 1e7;
double R = 10;
/****************************************
* Generate joint trajectory
***************************************/
gZU.setParameters( dt, 9.81, dofs );
gZU.generateZmpPositions( numSteps, true,
stepLength, footSeparation,
stepDuration,
slopeTime,
levelTime );
gZU.getControllerGains( Qe, R, zg, numPreviewSteps );
gZU.generateCOMPositions();
gZU.getJointTrajectories();
gZU.print( "jointsWholeBody.txt", gZU.mWholeBody );
gZU.mDartDofs;
joint_traj.insert(joint_traj.end(), gZU.mDartDofs.begin(), gZU.mDartDofs.end());
// Bake me some GUI viz
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(ATLAS_DATA_PATH"/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
vector<SkeletonDynamics *> skels;
skels.push_back(ground);
skels.push_back(_atlas);
ZmpGUI gui(skels);
gui.bake(joint_traj);
glutInit(&argc, argv);
gui.initWindow(640, 480, "Atlas ZMP Walking");
glutMainLoop();
/**************************************
* Publish joint trajectory
****************************************/
//FIXME: ROS Dependent
//MoveJointTractory(AK, _atlas, dofs, gZU.mWholeBody, 20, 20, 20, 1.2, 1.2, 1.2);
return 0;
}
void Enquire_MinPath(int src, int dst, Graph& g, Path& path)
{
double* d = new double[g.m_v]; //d[]距离数组，d[i]表示i到s的最短路长度
int* pre = new int[g.m_v]; //pre[]前驱数组，pre[i]表示i到s的最短路径中i的前驱
int* preW = new int[g.m_v]; //preW[]权值数组，preW[i]表示pre[i]->i的边的标号
bool* flag = new bool[g.m_v]; //flag[]标记数组，flag[i]==true表示i在点集S中
for(int i = 0;i < g.m_v;i++)
{
d[i] = -1; //由于不清楚w[i]范围，故用d[i]=-1表示i到s距离为无穷
pre[i] = src;
preW[i] = -1;
flag[i] = false;
}
d[src] = 0;
flag[src] = true;
Relax(g.m_p, g.m_q, g.m_r, src, pre, preW, d);
int minX = -1; //d[i]最小值的下标
for(int i = 0;i < g.m_v-1;i++)
{
//选取在点集S中d[]min，并更新
minX = -1;
for(int j = 0;j < g.m_v;j++)
if(g.m_hash[j])
if(!flag[j])
{
if(minX == -1)
minX = j;
else if(d[j] != -1)
{
if(d[minX] == -1)
minX = j;
else if(d[j] < d[minX])
minX = j;
}
}
if(minX == -1) break;
flag[minX] = true;
if(flag[dst])
break;
Relax(g.m_p , g.m_q, g.m_r, minX, pre, preW, d);
}
//道路不通
if(d[dst] == -1)
{
delete[] d;
delete[] pre;
delete[] flag;
delete[] preW;
return;
}
int* _path = new int[g.m_v+1]; //路径数组
int pathLen = 0; //路径长度
double pathW = 0; //路径权值
//根据pre,preW数组递归构造_path数组
path.m_epath = new int[g.m_v];
pathLen = 1;
_path[1] = dst;
while(dst != src)
{
pathW += g.m_r[preW[dst]];
path.m_epath[pathLen-1] = preW[dst];
dst = pre[dst];
pathLen++;
_path[pathLen] = dst;
}
path.m_epath[pathLen-1] = -1;
int temp = 0;
for(int i = 1;i <= pathLen / 2;i++)
{
temp = _path[i];
_path[i] = _path[pathLen - i + 1];
_path[pathLen - i + 1] = temp;
}
_path[pathLen+1] = -1;
delete[] d;
delete[] pre;
delete[] preW;
delete[] flag;
path.m_path = new int[pathLen];
for(int i = 0;i <= pathLen;i++)
path.m_path[i] = _path[i+1];
path.m_weight = pathW;
delete[] _path;
}
void Dijkstra::Execute(const Graph& graph, const string& vetexId)
{
m_Ret.PathTree.clear();
const auto &Vertexes = graph.GetVertexes();
Vertex* pVertexStart = Vertexes.find(vetexId)->second;
vector< Vertex* > Q;
//// 初始化
//for (auto& it : Vertexes)
//{
// it.second->PathfindingData.Cost = 0x0FFFFFFF;
// Q.push_back(it.second);
// pVertexStart->PathfindingData.pParent = nullptr;
//}
////m_Ret.PathTree[pVertexStart] = 0; // 起始顶点的前驱顶点为空
//pVertexStart->PathfindingData.Cost = 0;
//for (; Q.size() > 0;)
//{
// // 选出最小路径估计的顶点
// auto v = ExtractMin(Q);
// // 对所有的出边进行“松弛”
// const auto& EO = v->GetEdgesOut();
// for (auto& it : EO)
// {
// Edge* pEdge = it.second;
// Relax(v, pEdge->GetEndVertex(), pEdge->GetWeight());
// }
//}
// 初始化
for (auto& it : Vertexes)
{
it.second->PathfindingData.Cost = 0x0FFFFFFF;
pVertexStart->PathfindingData.pParent = nullptr;
}
// 初始化起始顶点
//m_Ret.PathTree[pVertexStart] = 0; // 起始顶点的前驱顶点为空
pVertexStart->PathfindingData.Cost = 0;
pVertexStart->PathfindingData.pParent = nullptr;
// 把起始顶点放入列表中
Q.push_back(pVertexStart);
pVertexStart->PathfindingData.Flag = true;
for (; Q.size() > 0;)
{
// 选出最小路径估计的顶点
auto v = ExtractMin(Q);
v->PathfindingData.Flag = false;
// 对所有的出边进行“松弛”
const auto& EO = v->GetEdgesOut();
for (auto& it : EO)
{
Edge* pEdge = it.second;
Vertex* pVEnd = pEdge->GetEndVertex();
bool bRel = Relax(v, pVEnd, pEdge->GetWeight());
if (bRel && pVEnd->PathfindingData.Flag == false)
{
Q.push_back(pVEnd);
pVEnd->PathfindingData.Flag = true;
}
}
}
}
void WdgGraph::Refresh(unsigned int _domainId) {
if (!solver.Running()) {
domainId = _domainId;
bool needsRelax = false, needsBoundsUpdate = false, filterActive = hiFilter.Active();
//
// refresh nodes
//
IgNodeMap nodeMap = iv->NodeMap(fm, domainId);
WgNodeVector::iterator node = nodes.begin();
while (node != nodes.end()) {
IgNodeMap::iterator nodeIt = nodeMap.find(node->id);
if (nodeIt != nodeMap.end()) {
node->selected = nodeIt->second.selected;
node->external = nodeIt->second.external;
node->multiuser = nodeIt->second.multiuser;
// row found, refresh node
if (*node != nodeIt->second) {
node->ts = nodeIt->second.ts;
node->title = QString::fromUtf8(nodeIt->second.text.c_str());
}
nodeMap.erase(nodeIt);
node++;
}
else {
// node id not found, delete node
if (node->hi.dir != hdNone)
--hilitNodes;
node = nodes.erase(node);
needsRelax = needsBoundsUpdate = true;
}
}
for (IgNodeMap::iterator nodeIt = nodeMap.begin(); nodeIt != nodeMap.end(); ++nodeIt) {
if (!filterActive || hiFilter.ContainsNode(nodeIt->first)) {
nodes.push_back(WgNode(
nodeIt->first,
nodeIt->second.ts,
nodeIt->second.selected,
nodeIt->second.external,
nodeIt->second.multiuser,
QString::fromUtf8(nodeIt->second.text.c_str())));
needsRelax = needsBoundsUpdate = true;
}
}
//
// refresh sub nodes
//
IgSubNodeMap subNodeMap = iv->SubNodeMap(fm, domainId);
WgSubNodeVector::iterator subNode = subNodes.begin();
while (subNode!= subNodes.end()) {
IgSubNodeMap::iterator subNodeIt = subNodeMap.find(subNode->id);
if (subNodeIt != subNodeMap.end()) {
subNode->selected = subNodeIt->second.selected;
subNode->external = subNodeIt->second.external;
// row found, refresh node
if (*subNode != subNodeIt->second) {
subNode->ts = subNodeIt->second.ts;
subNode->title = QString::fromUtf8(subNodeIt->second.text.c_str());
subNode->im.id = subNodeIt->second.im;
}
subNodeMap.erase(subNodeIt);
subNode++;
}
else {
// node id not found, delete node
subNode = subNodes.erase(subNode);
needsRelax = needsBoundsUpdate = true;
}
}
for (IgSubNodeMap::iterator subNodeIt = subNodeMap.begin(); subNodeIt != subNodeMap.end(); ++subNodeIt) {
if (!filterActive || hiFilter.ContainsSubNode(subNodeIt->first)) {
subNodes.push_back(WgSubNode(
subNodeIt->first,
subNodeIt->second.ts,
subNodeIt->second.selected,
subNodeIt->second.external,
QString::fromUtf8(subNodeIt->second.text.c_str()),
subNodeIt->second.im));
needsRelax = needsBoundsUpdate = true;
}
}
if (needsBoundsUpdate)
UpdateBounds();
//
// refresh links
//
IgLinkMap linkMap = iv->LinkMap(fm, domainId);
WgLinkList::iterator link = links.begin();
while (link != links.end()) {
IgLinkMap::iterator linkIt = linkMap.find(link->id);
if (linkIt != linkMap.end()) {
link->selected = linkIt->second.selected;
// row found, refresh link
if (*link != linkIt->second) {
link->ts = linkIt->second.ts;
link->us.id = linkIt->second.us;
link->im.id = linkIt->second.im;
link->sb.id = linkIt->second.sb;
needsRelax = true;
}
linkMap.erase(linkIt);
link++;
}
else {
// link id not found, delete link
link = links.erase(link);
needsRelax = true;
}
}
for (IgLinkMap::iterator linkIt = linkMap.begin(); linkIt != linkMap.end(); ++linkIt) {
if (!filterActive || hiFilter.ContainsLink(linkIt->first)) {
links.push_back(WgLink(
linkIt->first,
linkIt->second.ts,
linkIt->second.selected,
linkIt->second.us,
linkIt->second.im,
linkIt->second.sb));
needsRelax = true;
}
}
//
// if modified, relax
//
if (needsRelax)
Relax();
updateGL();
}
}
