<commit_before>//===----------------------------------------------------------------------===//
//
//                         PelotonDB
//
// hash_join_test.cpp
//
// Identification: tests/executor/hash_join_test.cpp
//
// Copyright (c) 2015, Carnegie Mellon University Database Group
//
//===----------------------------------------------------------------------===//

#include <memory>

#include "gmock/gmock.h"
#include "gtest/gtest.h"

#include "backend/common/types.h"
#include "backend/executor/logical_tile.h"
#include "backend/executor/logical_tile_factory.h"

#include "backend/executor/hash_join_executor.h"
#include "backend/executor/hash_executor.h"
#include "backend/executor/merge_join_executor.h"
#include "backend/executor/nested_loop_join_executor.h"

#include "backend/expression/abstract_expression.h"
#include "backend/expression/tuple_value_expression.h"
#include "backend/expression/expression_util.h"

#include "backend/planner/hash_join_plan.h"
#include "backend/planner/hash_plan.h"
#include "backend/planner/merge_join_plan.h"
#include "backend/planner/nested_loop_join_plan.h"

#include "backend/storage/data_table.h"

#include "mock_executor.h"
#include "executor/executor_tests_util.h"
#include "executor/join_tests_util.h"
#include "harness.h"

using ::testing::NotNull;
using ::testing::Return;

namespace peloton {
namespace test {

std::vector<planner::MergeJoinPlan::JoinClause> CreateJoinClauses() {
  std::vector<planner::MergeJoinPlan::JoinClause> join_clauses;
  auto left = expression::TupleValueFactory(0, 1);
  auto right = expression::TupleValueFactory(1, 1);
  bool reversed = false;
  join_clauses.emplace_back(left, right, reversed);
  return join_clauses;
}

std::vector<PlanNodeType> join_algorithms = {
    PLAN_NODE_TYPE_NESTLOOP,
    PLAN_NODE_TYPE_MERGEJOIN,
    PLAN_NODE_TYPE_HASHJOIN
};

std::vector<PelotonJoinType> join_types = {
    JOIN_TYPE_INNER,
    JOIN_TYPE_LEFT,
    JOIN_TYPE_RIGHT,
    JOIN_TYPE_OUTER
};

void ExecuteJoinTest(PlanNodeType join_algorithm, PelotonJoinType join_type, bool compute_cartesian_product);

TEST(JoinTests, JoinPredicateTest) {

  const bool compute_cartesian_product = false;

  // Go over all join algorithms
  for(auto join_algorithm : join_algorithms) {
    std::cout << "JOIN ALGORITHM :: " << PlanNodeTypeToString(join_algorithm) << "\n";

    // Go over all join types
    for(auto join_type : join_types) {
      std::cout << "JOIN TYPE :: " << join_type << "\n";

      // Execute the join test
      ExecuteJoinTest(join_algorithm, join_type, compute_cartesian_product);

    }
  }

}

TEST(JoinTests, CartesianProductTest) {

  const bool compute_cartesian_product = true;

  // Go over all join algorithms
  for(auto join_algorithm : join_algorithms) {
    std::cout << "JOIN ALGORITHM :: " << PlanNodeTypeToString(join_algorithm) << "\n";

    // Go over all join types
    for(auto join_type : join_types) {
      std::cout << "JOIN TYPE :: " << join_type << "\n";

      // Execute the join test
      ExecuteJoinTest(join_algorithm, join_type, compute_cartesian_product);

    }
  }

}

void ExecuteJoinTest(PlanNodeType join_algorithm, PelotonJoinType join_type, __attribute__((unused)) bool cartesian_product) {

  //===--------------------------------------------------------------------===//
  // Mock table scan executors
  //===--------------------------------------------------------------------===//

  MockExecutor left_table_scan_executor, right_table_scan_executor;

  // Create a table and wrap it in logical tile
  size_t tile_group_size = TESTS_TUPLES_PER_TILEGROUP;
  size_t left_table_tile_group_count = 3;
  size_t right_table_tile_group_count = 2;

  // Left table has 3 tile groups
  std::unique_ptr<storage::DataTable> left_table(
      ExecutorTestsUtil::CreateTable(tile_group_size));
  ExecutorTestsUtil::PopulateTable(left_table.get(),
                                   tile_group_size * left_table_tile_group_count,
                                   false,
                                   false, false);

  //std::cout << (*left_table);

  // Right table has 2 tile groups
  std::unique_ptr<storage::DataTable> right_table(
      ExecutorTestsUtil::CreateTable(tile_group_size));
  ExecutorTestsUtil::PopulateTable(right_table.get(),
                                   tile_group_size * right_table_tile_group_count,
                                   false, false, false);

  //std::cout << (*right_table);

  // Wrap the input tables with logical tiles
  std::unique_ptr<executor::LogicalTile> left_table_logical_tile1(
      executor::LogicalTileFactory::WrapTileGroup(left_table->GetTileGroup(0)));
  std::unique_ptr<executor::LogicalTile> left_table_logical_tile2(
      executor::LogicalTileFactory::WrapTileGroup(left_table->GetTileGroup(1)));
  std::unique_ptr<executor::LogicalTile> left_table_logical_tile3(
      executor::LogicalTileFactory::WrapTileGroup(left_table->GetTileGroup(2)));

  std::unique_ptr<executor::LogicalTile> right_table_logical_tile1(
      executor::LogicalTileFactory::WrapTileGroup(
          right_table->GetTileGroup(0)));
  std::unique_ptr<executor::LogicalTile> right_table_logical_tile2(
      executor::LogicalTileFactory::WrapTileGroup(
          right_table->GetTileGroup(1)));

  // Left scan executor returns logical tiles from the left table

  EXPECT_CALL(left_table_scan_executor, DInit())
  .WillOnce(Return(true));

  EXPECT_CALL(left_table_scan_executor, DExecute())
  .WillOnce(Return(true))
  .WillOnce(Return(true))
  .WillOnce(Return(true))
  .WillOnce(Return(false));

  EXPECT_CALL(left_table_scan_executor, GetOutput())
  .WillOnce(Return(left_table_logical_tile1.release()))
  .WillOnce(Return(left_table_logical_tile2.release()))
  .WillOnce(Return(left_table_logical_tile3.release()));

  // Right scan executor returns logical tiles from the right table

  EXPECT_CALL(right_table_scan_executor, DInit())
  .WillOnce(Return(true));

  EXPECT_CALL(right_table_scan_executor, DExecute())
  .WillOnce(Return(true))
  .WillOnce(Return(true))
  .WillOnce(Return(false));

  EXPECT_CALL(right_table_scan_executor, GetOutput())
  .WillOnce(Return(right_table_logical_tile1.release()))
  .WillOnce(Return(right_table_logical_tile2.release()));

  //===--------------------------------------------------------------------===//
  // Setup join plan nodes and executors and run them
  //===--------------------------------------------------------------------===//

  auto result_tuple_count = 0;
  auto projection = JoinTestsUtil::CreateProjection();

  // Differ based on join algorithm
  switch(join_algorithm) {

    case PLAN_NODE_TYPE_NESTLOOP: {

      // Construct predicate
      expression::AbstractExpression *predicate = nullptr;
      predicate = JoinTestsUtil::CreateJoinPredicate();

      // Create nested loop join plan node.
      planner::NestedLoopJoinPlan nested_loop_join_node(join_type, predicate, projection);

      // Run the nested loop join executor
      executor::NestedLoopJoinExecutor nested_loop_join_executor(&nested_loop_join_node, nullptr);

      // Construct the executor tree
      nested_loop_join_executor.AddChild(&left_table_scan_executor);
      nested_loop_join_executor.AddChild(&right_table_scan_executor);

      // Run the nested loop join executor
      EXPECT_TRUE(nested_loop_join_executor.Init());
      for(oid_t execution_itr = 0 ; execution_itr < 4; execution_itr++) {
        nested_loop_join_executor.Execute();
        std::unique_ptr<executor::LogicalTile> result_logical_tile(nested_loop_join_executor.GetOutput());

        if(result_logical_tile != nullptr) {
          result_tuple_count += result_logical_tile->GetTupleCount();
        }
      }

    }
    break;

    case PLAN_NODE_TYPE_MERGEJOIN: {

      // Create join clauses
      std::vector<planner::MergeJoinPlan::JoinClause> join_clauses;
      join_clauses = CreateJoinClauses();

      // Create merge join plan node
      planner::MergeJoinPlan merge_join_node(join_type, nullptr, projection, join_clauses);

      // Construct the merge join executor
      executor::MergeJoinExecutor merge_join_executor(&merge_join_node, nullptr);

      // Construct the executor tree
      merge_join_executor.AddChild(&left_table_scan_executor);
      merge_join_executor.AddChild(&right_table_scan_executor);

      // Run the merge join executor
      EXPECT_TRUE(merge_join_executor.Init());
      for(oid_t execution_itr = 0 ; execution_itr < 4; execution_itr++) {
        merge_join_executor.Execute();
        std::unique_ptr<executor::LogicalTile> result_logical_tile(merge_join_executor.GetOutput());

        if(result_logical_tile != nullptr) {
          result_tuple_count += result_logical_tile->GetTupleCount();
          //std::cout << (*result_logical_tile);
        }
      }

    }
    break;

    case PLAN_NODE_TYPE_HASHJOIN: {

      // Create hash plan node
      expression::AbstractExpression *right_table_attr_1 =
          new expression::TupleValueExpression(1, 1);

      std::vector<std::unique_ptr<const expression::AbstractExpression> > hash_keys;
      hash_keys.emplace_back(right_table_attr_1);

      // Create hash plan node
      planner::HashPlan hash_plan_node(hash_keys);

      // Construct the hash executor
      executor::HashExecutor hash_executor(&hash_plan_node, nullptr);

      // Create hash join plan node.
      planner::HashJoinPlan hash_join_plan_node(join_type, nullptr, projection);

      // Construct the hash join executor
      executor::HashJoinExecutor hash_join_executor(&hash_join_plan_node, nullptr);

      // Construct the executor tree
      hash_join_executor.AddChild(&left_table_scan_executor);
      hash_join_executor.AddChild(&hash_executor);

      hash_executor.AddChild(&right_table_scan_executor);

      // Run the hash_join_executor
      EXPECT_TRUE(hash_join_executor.Init());
      for(oid_t execution_itr = 0 ; execution_itr < 4; execution_itr++) {
        hash_join_executor.Execute();
        std::unique_ptr<executor::LogicalTile> result_logical_tile(hash_join_executor.GetOutput());

        if(result_logical_tile != nullptr) {
          result_tuple_count += result_logical_tile->GetTupleCount();
          //std::cout << (*result_logical_tile);
        }
      }

    }
    break;

    default:
      throw Exception("Unsupported join algorithm : " + std::to_string(join_algorithm));
      break;
  }

  //===--------------------------------------------------------------------===//
  // Execute test
  //===--------------------------------------------------------------------===//

  // Check output
  switch(join_type) {
    case JOIN_TYPE_INNER:
      EXPECT_EQ(result_tuple_count, 2 * tile_group_size);
      break;

    case JOIN_TYPE_LEFT:
      EXPECT_EQ(result_tuple_count, 3 * tile_group_size);
      break;

    case JOIN_TYPE_RIGHT:
      EXPECT_EQ(result_tuple_count, 2 * tile_group_size);
      break;

    case JOIN_TYPE_OUTER:
      EXPECT_EQ(result_tuple_count, 3 * tile_group_size);
      break;

    default:
      throw Exception("Unsupported join type : " + std::to_string(join_type));
      break;
  }

}

}  // namespace test
}  // namespace peloton
<commit_msg>Checkpoint<commit_after>//===----------------------------------------------------------------------===//
//
//                         PelotonDB
//
// hash_join_test.cpp
//
// Identification: tests/executor/hash_join_test.cpp
//
// Copyright (c) 2015, Carnegie Mellon University Database Group
//
//===----------------------------------------------------------------------===//

#include <memory>

#include "gmock/gmock.h"
#include "gtest/gtest.h"

#include "backend/common/types.h"
#include "backend/executor/logical_tile.h"
#include "backend/executor/logical_tile_factory.h"

#include "backend/executor/hash_join_executor.h"
#include "backend/executor/hash_executor.h"
#include "backend/executor/merge_join_executor.h"
#include "backend/executor/nested_loop_join_executor.h"

#include "backend/expression/abstract_expression.h"
#include "backend/expression/tuple_value_expression.h"
#include "backend/expression/expression_util.h"

#include "backend/planner/hash_join_plan.h"
#include "backend/planner/hash_plan.h"
#include "backend/planner/merge_join_plan.h"
#include "backend/planner/nested_loop_join_plan.h"

#include "backend/storage/data_table.h"

#include "mock_executor.h"
#include "executor/executor_tests_util.h"
#include "executor/join_tests_util.h"
#include "harness.h"

using ::testing::NotNull;
using ::testing::Return;

namespace peloton {
namespace test {

std::vector<planner::MergeJoinPlan::JoinClause> CreateJoinClauses() {
  std::vector<planner::MergeJoinPlan::JoinClause> join_clauses;
  auto left = expression::TupleValueFactory(0, 1);
  auto right = expression::TupleValueFactory(1, 1);
  bool reversed = false;
  join_clauses.emplace_back(left, right, reversed);
  return join_clauses;
}

std::vector<PlanNodeType> join_algorithms = {
    PLAN_NODE_TYPE_NESTLOOP,
    PLAN_NODE_TYPE_MERGEJOIN
};

std::vector<PelotonJoinType> join_types = {
    JOIN_TYPE_INNER,
    JOIN_TYPE_LEFT,
    JOIN_TYPE_RIGHT,
    JOIN_TYPE_OUTER
};

void ExecuteJoinTest(PlanNodeType join_algorithm, PelotonJoinType join_type, bool compute_cartesian_product);

TEST(JoinTests, JoinPredicateTest) {

  const bool compute_cartesian_product = false;

  // Go over all join algorithms
  for(auto join_algorithm : join_algorithms) {
    std::cout << "JOIN ALGORITHM :: " << PlanNodeTypeToString(join_algorithm) << "\n";

    // Go over all join types
    for(auto join_type : join_types) {
      std::cout << "JOIN TYPE :: " << join_type << "\n";

      // Execute the join test
      ExecuteJoinTest(join_algorithm, join_type, compute_cartesian_product);

    }
  }

}

TEST(JoinTests, CartesianProductTest) {

  const bool compute_cartesian_product = true;

  // Go over all join algorithms
  for(auto join_algorithm : join_algorithms) {
    std::cout << "JOIN ALGORITHM :: " << PlanNodeTypeToString(join_algorithm) << "\n";

    // Go over all join types
    for(auto join_type : join_types) {
      std::cout << "JOIN TYPE :: " << join_type << "\n";

      // Execute the join test
      ExecuteJoinTest(join_algorithm, join_type, compute_cartesian_product);

    }
  }

}

void ExecuteJoinTest(PlanNodeType join_algorithm, PelotonJoinType join_type, __attribute__((unused)) bool cartesian_product) {

  //===--------------------------------------------------------------------===//
  // Mock table scan executors
  //===--------------------------------------------------------------------===//

  MockExecutor left_table_scan_executor, right_table_scan_executor;

  // Create a table and wrap it in logical tile
  size_t tile_group_size = TESTS_TUPLES_PER_TILEGROUP;
  size_t left_table_tile_group_count = 3;
  size_t right_table_tile_group_count = 2;

  // Left table has 3 tile groups
  std::unique_ptr<storage::DataTable> left_table(
      ExecutorTestsUtil::CreateTable(tile_group_size));
  ExecutorTestsUtil::PopulateTable(left_table.get(),
                                   tile_group_size * left_table_tile_group_count,
                                   false,
                                   false, false);

  //std::cout << (*left_table);

  // Right table has 2 tile groups
  std::unique_ptr<storage::DataTable> right_table(
      ExecutorTestsUtil::CreateTable(tile_group_size));
  ExecutorTestsUtil::PopulateTable(right_table.get(),
                                   tile_group_size * right_table_tile_group_count,
                                   false, false, false);

  //std::cout << (*right_table);

  // Wrap the input tables with logical tiles
  std::unique_ptr<executor::LogicalTile> left_table_logical_tile1(
      executor::LogicalTileFactory::WrapTileGroup(left_table->GetTileGroup(0)));
  std::unique_ptr<executor::LogicalTile> left_table_logical_tile2(
      executor::LogicalTileFactory::WrapTileGroup(left_table->GetTileGroup(1)));
  std::unique_ptr<executor::LogicalTile> left_table_logical_tile3(
      executor::LogicalTileFactory::WrapTileGroup(left_table->GetTileGroup(2)));

  std::unique_ptr<executor::LogicalTile> right_table_logical_tile1(
      executor::LogicalTileFactory::WrapTileGroup(
          right_table->GetTileGroup(0)));
  std::unique_ptr<executor::LogicalTile> right_table_logical_tile2(
      executor::LogicalTileFactory::WrapTileGroup(
          right_table->GetTileGroup(1)));

  // Left scan executor returns logical tiles from the left table

  EXPECT_CALL(left_table_scan_executor, DInit())
  .WillOnce(Return(true));

  EXPECT_CALL(left_table_scan_executor, DExecute())
  .WillOnce(Return(true))
  .WillOnce(Return(true))
  .WillOnce(Return(true))
  .WillOnce(Return(false));

  EXPECT_CALL(left_table_scan_executor, GetOutput())
  .WillOnce(Return(left_table_logical_tile1.release()))
  .WillOnce(Return(left_table_logical_tile2.release()))
  .WillOnce(Return(left_table_logical_tile3.release()));

  // Right scan executor returns logical tiles from the right table

  EXPECT_CALL(right_table_scan_executor, DInit())
  .WillOnce(Return(true));

  EXPECT_CALL(right_table_scan_executor, DExecute())
  .WillOnce(Return(true))
  .WillOnce(Return(true))
  .WillOnce(Return(false));

  EXPECT_CALL(right_table_scan_executor, GetOutput())
  .WillOnce(Return(right_table_logical_tile1.release()))
  .WillOnce(Return(right_table_logical_tile2.release()));

  //===--------------------------------------------------------------------===//
  // Setup join plan nodes and executors and run them
  //===--------------------------------------------------------------------===//

  auto result_tuple_count = 0;
  auto projection = JoinTestsUtil::CreateProjection();

  // Differ based on join algorithm
  switch(join_algorithm) {

    case PLAN_NODE_TYPE_NESTLOOP: {

      // Construct predicate
      expression::AbstractExpression *predicate = nullptr;
      predicate = JoinTestsUtil::CreateJoinPredicate();

      // Create nested loop join plan node.
      planner::NestedLoopJoinPlan nested_loop_join_node(join_type, predicate, projection);

      // Run the nested loop join executor
      executor::NestedLoopJoinExecutor nested_loop_join_executor(&nested_loop_join_node, nullptr);

      // Construct the executor tree
      nested_loop_join_executor.AddChild(&left_table_scan_executor);
      nested_loop_join_executor.AddChild(&right_table_scan_executor);

      // Run the nested loop join executor
      EXPECT_TRUE(nested_loop_join_executor.Init());
      for(oid_t execution_itr = 0 ; execution_itr < 4; execution_itr++) {
        nested_loop_join_executor.Execute();
        std::unique_ptr<executor::LogicalTile> result_logical_tile(nested_loop_join_executor.GetOutput());

        if(result_logical_tile != nullptr) {
          result_tuple_count += result_logical_tile->GetTupleCount();
        }
      }

    }
    break;

    case PLAN_NODE_TYPE_MERGEJOIN: {

      // Create join clauses
      std::vector<planner::MergeJoinPlan::JoinClause> join_clauses;
      join_clauses = CreateJoinClauses();

      // Create merge join plan node
      planner::MergeJoinPlan merge_join_node(join_type, nullptr, projection, join_clauses);

      // Construct the merge join executor
      executor::MergeJoinExecutor merge_join_executor(&merge_join_node, nullptr);

      // Construct the executor tree
      merge_join_executor.AddChild(&left_table_scan_executor);
      merge_join_executor.AddChild(&right_table_scan_executor);

      // Run the merge join executor
      EXPECT_TRUE(merge_join_executor.Init());
      for(oid_t execution_itr = 0 ; execution_itr < 4; execution_itr++) {
        merge_join_executor.Execute();
        std::unique_ptr<executor::LogicalTile> result_logical_tile(merge_join_executor.GetOutput());

        if(result_logical_tile != nullptr) {
          result_tuple_count += result_logical_tile->GetTupleCount();
          //std::cout << (*result_logical_tile);
        }
      }

    }
    break;

    case PLAN_NODE_TYPE_HASHJOIN: {

      // Create hash plan node
      expression::AbstractExpression *right_table_attr_1 =
          new expression::TupleValueExpression(1, 1);

      std::vector<std::unique_ptr<const expression::AbstractExpression> > hash_keys;
      hash_keys.emplace_back(right_table_attr_1);

      // Create hash plan node
      planner::HashPlan hash_plan_node(hash_keys);

      // Construct the hash executor
      executor::HashExecutor hash_executor(&hash_plan_node, nullptr);

      // Create hash join plan node.
      planner::HashJoinPlan hash_join_plan_node(join_type, nullptr, projection);

      // Construct the hash join executor
      executor::HashJoinExecutor hash_join_executor(&hash_join_plan_node, nullptr);

      // Construct the executor tree
      hash_join_executor.AddChild(&left_table_scan_executor);
      hash_join_executor.AddChild(&hash_executor);

      hash_executor.AddChild(&right_table_scan_executor);

      // Run the hash_join_executor
      EXPECT_TRUE(hash_join_executor.Init());
      for(oid_t execution_itr = 0 ; execution_itr < 4; execution_itr++) {
        hash_join_executor.Execute();
        std::unique_ptr<executor::LogicalTile> result_logical_tile(hash_join_executor.GetOutput());

        if(result_logical_tile != nullptr) {
          result_tuple_count += result_logical_tile->GetTupleCount();
          //std::cout << (*result_logical_tile);
        }
      }

    }
    break;

    default:
      throw Exception("Unsupported join algorithm : " + std::to_string(join_algorithm));
      break;
  }

  //===--------------------------------------------------------------------===//
  // Execute test
  //===--------------------------------------------------------------------===//

  // Check output
  switch(join_type) {
    case JOIN_TYPE_INNER:
      EXPECT_EQ(result_tuple_count, 2 * tile_group_size);
      break;

    case JOIN_TYPE_LEFT:
      EXPECT_EQ(result_tuple_count, 3 * tile_group_size);
      break;

    case JOIN_TYPE_RIGHT:
      EXPECT_EQ(result_tuple_count, 2 * tile_group_size);
      break;

    case JOIN_TYPE_OUTER:
      EXPECT_EQ(result_tuple_count, 3 * tile_group_size);
      break;

    default:
      throw Exception("Unsupported join type : " + std::to_string(join_type));
      break;
  }

}

}  // namespace test
}  // namespace peloton
<|endoftext|>


<commit_before>/* $Id: middlewareGeneralTests.cpp,v 1.1.2.7 2013/02/01 17:26:58 matthewmulhern Exp $
 *
 * OpenMAMA: The open middleware agnostic messaging API
 * Copyright (C) 2011 NYSE Technologies, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301 USA
 */

#include <gtest/gtest.h>
#include "mama/mama.h"
#include "MainUnitTestC.h"
#include <iostream>
#include "bridge.h"
#include "mama/types.h"

using std::cout;
using std::endl;

class MiddlewareGeneralTests : public ::testing::Test
{
protected:
    MiddlewareGeneralTests(void);
    virtual ~MiddlewareGeneralTests(void);

    virtual void SetUp(void);
    virtual void TearDown(void);

    mamaBridge mBridge;
};

MiddlewareGeneralTests::MiddlewareGeneralTests(void)
{
}

MiddlewareGeneralTests::~MiddlewareGeneralTests(void)
{
}

void MiddlewareGeneralTests::SetUp(void)
{
	mama_loadBridge (&mBridge,getMiddleware());
}

void MiddlewareGeneralTests::TearDown(void)
{
}

/*===================================================================
 =                      Used in mama.c                              =
 ====================================================================*/
/* TODO: Rewrite this test to call bridgeStart on a separate thread, since it is
 * a blocking call. Otherwise, bridgeStop is never called.
 *
 */
TEST_F (MiddlewareGeneralTests, DISABLED_startStop)
{
    mamaQueue defaultEventQueue = NULL;

    ASSERT_EQ (MAMA_STATUS_OK,
               mama_getDefaultEventQueue(mBridge, &defaultEventQueue));

    ASSERT_EQ (MAMA_STATUS_OK,
               mBridge->bridgeStart(defaultEventQueue));

    ASSERT_EQ (MAMA_STATUS_OK,
               mBridge->bridgeStop(defaultEventQueue));
}

TEST_F (MiddlewareGeneralTests, startInvalid)
{
    ASSERT_EQ (MAMA_STATUS_NULL_ARG, mBridge->bridgeStart(NULL));
}

TEST_F (MiddlewareGeneralTests, stopInvalid)
{
    ASSERT_EQ (MAMA_STATUS_NULL_ARG, mBridge->bridgeStop(NULL));
}

TEST_F (MiddlewareGeneralTests, getVersion)
{
    const char* res = "0.0";

    res = mBridge->bridgeGetVersion();

	ASSERT_TRUE (res != NULL);
}

TEST_F (MiddlewareGeneralTests, getName)
{
    const char* res = "name";

    res = mBridge->bridgeGetName();

	ASSERT_TRUE (res != NULL);
}

TEST_F (MiddlewareGeneralTests, getDefaultPayloadId)
{
    char** names = {NULL};
    char*  id    = NULL;

    ASSERT_EQ (MAMA_STATUS_OK,
               mBridge-> bridgeGetDefaultPayloadId(&names,&id));
}

TEST_F (MiddlewareGeneralTests, getDefaultPayloadIdInvalidName)
{
    char* id = NULL;

    ASSERT_EQ (MAMA_STATUS_NULL_ARG,
               mBridge-> bridgeGetDefaultPayloadId(NULL,&id));
}

TEST_F (MiddlewareGeneralTests, getDefaultPayloadIdInvalidId)
{
    char ** names;

    ASSERT_EQ (MAMA_STATUS_NULL_ARG,
               mBridge-> bridgeGetDefaultPayloadId(&names,NULL));
}

<commit_msg>UNITTEST: Fixed leak in middleware general unit test<commit_after>/* $Id: middlewareGeneralTests.cpp,v 1.1.2.7 2013/02/01 17:26:58 matthewmulhern Exp $
 *
 * OpenMAMA: The open middleware agnostic messaging API
 * Copyright (C) 2011 NYSE Technologies, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301 USA
 */

#include <gtest/gtest.h>
#include "mama/mama.h"
#include "MainUnitTestC.h"
#include <iostream>
#include "bridge.h"
#include "mama/types.h"

using std::cout;
using std::endl;

class MiddlewareGeneralTests : public ::testing::Test
{
protected:
    MiddlewareGeneralTests(void);
    virtual ~MiddlewareGeneralTests(void);

    virtual void SetUp(void);
    virtual void TearDown(void);

    mamaBridge mBridge;
};

MiddlewareGeneralTests::MiddlewareGeneralTests(void)
{
}

MiddlewareGeneralTests::~MiddlewareGeneralTests(void)
{
}

void MiddlewareGeneralTests::SetUp(void)
{
    mama_loadBridge (&mBridge,getMiddleware());
    mama_open ();
}

void MiddlewareGeneralTests::TearDown(void)
{
    mama_close ();
}

/*===================================================================
 =                      Used in mama.c                              =
 ====================================================================*/
/* TODO: Rewrite this test to call bridgeStart on a separate thread, since it is
 * a blocking call. Otherwise, bridgeStop is never called.
 *
 */
TEST_F (MiddlewareGeneralTests, DISABLED_startStop)
{
    mamaQueue defaultEventQueue = NULL;

    ASSERT_EQ (MAMA_STATUS_OK,
               mama_getDefaultEventQueue(mBridge, &defaultEventQueue));

    ASSERT_EQ (MAMA_STATUS_OK,
               mBridge->bridgeStart(defaultEventQueue));

    ASSERT_EQ (MAMA_STATUS_OK,
               mBridge->bridgeStop(defaultEventQueue));
}

TEST_F (MiddlewareGeneralTests, startInvalid)
{
    ASSERT_EQ (MAMA_STATUS_NULL_ARG, mBridge->bridgeStart(NULL));
}

TEST_F (MiddlewareGeneralTests, stopInvalid)
{
    ASSERT_EQ (MAMA_STATUS_NULL_ARG, mBridge->bridgeStop(NULL));
}

TEST_F (MiddlewareGeneralTests, getVersion)
{
    const char* res = "0.0";

    res = mBridge->bridgeGetVersion();

	ASSERT_TRUE (res != NULL);
}

TEST_F (MiddlewareGeneralTests, getName)
{
    const char* res = "name";

    res = mBridge->bridgeGetName();

	ASSERT_TRUE (res != NULL);
}

TEST_F (MiddlewareGeneralTests, getDefaultPayloadId)
{
    char** names = {NULL};
    char*  id    = NULL;

    ASSERT_EQ (MAMA_STATUS_OK,
               mBridge-> bridgeGetDefaultPayloadId(&names,&id));
}

TEST_F (MiddlewareGeneralTests, getDefaultPayloadIdInvalidName)
{
    char* id = NULL;

    ASSERT_EQ (MAMA_STATUS_NULL_ARG,
               mBridge-> bridgeGetDefaultPayloadId(NULL,&id));
}

TEST_F (MiddlewareGeneralTests, getDefaultPayloadIdInvalidId)
{
    char ** names;

    ASSERT_EQ (MAMA_STATUS_NULL_ARG,
               mBridge-> bridgeGetDefaultPayloadId(&names,NULL));
}

<|endoftext|>


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: E. Gil Jones */

#include <ros/ros.h>
#include <chomp_motion_planner/chomp_planner.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <chomp_motion_planner/chomp_optimizer.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/MotionPlanRequest.h>

namespace chomp
{
ChompPlanner::ChompPlanner()
{
}

bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
                         moveit_msgs::MotionPlanDetailedResponse& res) const
{
  if (!planning_scene)
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
    return false;
  }

  if (req.start_state.joint_state.position.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  ros::WallTime start_time = ros::WallTime::now();
  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);

  jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
                    trajectory.getTrajectoryPoint(0));

  if (req.goal_constraints.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  if (req.goal_constraints[0].joint_constraints.empty())
  {
    ROS_ERROR_STREAM("Only joint-space goals are supported");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  int goal_index = trajectory.getNumPoints() - 1;
  trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
  sensor_msgs::JointState js;
  for (unsigned int i = 0; i < req.goal_constraints[0].joint_constraints.size(); i++)
  {
    js.name.push_back(req.goal_constraints[0].joint_constraints[i].joint_name);
    js.position.push_back(req.goal_constraints[0].joint_constraints[i].position);
    ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << req.goal_constraints[0].joint_constraints[i].joint_name
                                                            << " to position "
                                                            << req.goal_constraints[0].joint_constraints[i].position);
  }
  jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));

  const moveit::core::JointModelGroup* model_group =
      planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
  // fix the goal to move the shortest angular distance for wrap-around joints:
  for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
  {
    const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
    const moveit::core::RevoluteJointModel* revolute_joint =
        dynamic_cast<const moveit::core::RevoluteJointModel*>(model);

    if (revolute_joint != NULL)
    {
      if (revolute_joint->isContinuous())
      {
        double start = (trajectory)(0, i);
        double end = (trajectory)(goal_index, i);
        ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
        (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
      }
    }
  }

  const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
  const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
  moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
  goal_robot_state.setVariablePositions(
      active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));

  if (not goal_robot_state.satisfiesBounds())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  // fill in an initial trajectory based on user choice from the chomp_config.yaml file
  if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
    trajectory.fillInMinJerk();
  else if (params.trajectory_initialization_method_.compare("linear") == 0)
    trajectory.fillInLinearInterpolation();
  else if (params.trajectory_initialization_method_.compare("cubic") == 0)
    trajectory.fillInCubicInterpolation();
  else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
  {
    if (!(trajectory.fillInFromTrajectory(res)))
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, "
                                              "trajectory must contain at least start and goal state");
      return false;
    }
  }
  else
    ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");

  ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
                 (params.trajectory_initialization_method_).c_str());

  // optimize!
  moveit::core::RobotState start_state(planning_scene->getCurrentState());
  moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
  start_state.update();

  ros::WallTime create_time = ros::WallTime::now();

  int replan_count = 0;
  bool replan_flag = false;
  double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10;
  int org_max_iterations = 200;

  // storing the initial chomp parameters values
  org_learning_rate = params.learning_rate_;
  org_ridge_factor = params.ridge_factor_;
  org_planning_time_limit = params.planning_time_limit_;
  org_max_iterations = params.max_iterations_;

  ChompOptimizer* optimizer;

  // create a non_const_params variable which stores the non constant version of the const params variable
  ChompParameters params_nonconst = params;

  // while loop for replanning (recovery behaviour) if collision free optimized solution not found
  while (true)
  {
    if (replan_flag)
    {
      // increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5
      // additional secs in hope to find a solution; increase maximum iterations
      params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
                                        params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
    }

    // initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
    // case of a recovery behaviour
    optimizer = new ChompOptimizer(&trajectory, planning_scene, req.group_name, &params_nonconst, start_state);
    if (!optimizer->isInitialized())
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer");
      res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      return false;
    }

    ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create",
                    (ros::WallTime::now() - create_time).toSec());

    bool optimization_result = optimizer->optimize();

    // replan with updated parameters if no solution is found
    if (params_nonconst.enable_failure_recovery_)
    {
      ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, "
                                      "planning_time_limit, max_iterations), attempt: # %d ",
                     (replan_count + 1));
      ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",
                     params_nonconst.learning_rate_, params_nonconst.ridge_factor_,
                     params_nonconst.planning_time_limit_, params_nonconst.max_iterations_);

      if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_)
      {
        replan_count++;
        replan_flag = true;
        // delete ChompOptimizer object 'optimizer' to prevent a possible memory leak
        delete optimizer;
      }
      else
      {
        break;
      }
    }
    else
      break;
  }  // end of while loop

  // resetting the CHOMP Parameters to the original values after a successful plan
  params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations);

  ROS_DEBUG_NAMED("chomp_planner", "Optimization actually took %f sec to run",
                  (ros::WallTime::now() - create_time).toSec());
  create_time = ros::WallTime::now();
  // assume that the trajectory is now optimized, fill in the output structure:

  ROS_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints());

  res.trajectory.resize(1);

  res.trajectory[0].joint_trajectory.joint_names = active_joint_names;

  res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header;  // @TODO this is probably a hack

  // fill in the entire trajectory
  res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
  for (int i = 0; i < trajectory.getNumPoints(); i++)
  {
    res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
    for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
    {
      res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
    }
    // Setting invalid timestamps.
    // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
    res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
  }

  ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f",
                  (ros::WallTime::now() - start_time).toSec(),
                  res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());

  // report planning failure if path has collisions
  if (not optimizer->isCollisionFree())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // check that final state is within goal tolerances
  kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
  robot_state::RobotState last_state(planning_scene->getRobotModel());
  last_state.setVariablePositions(res.trajectory[0].joint_trajectory.points.back().positions.data());

  bool constraints_are_ok = true;
  for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
  {
    constraints_are_ok = constraints_are_ok and jc.configure(constraint);
    constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
    if (not constraints_are_ok)
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name);
      res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
      return false;
    }
  }

  return true;
}
}
<commit_msg>set last_state only for active joints in chomp_planner (#1222)<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: E. Gil Jones */

#include <ros/ros.h>
#include <chomp_motion_planner/chomp_planner.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <chomp_motion_planner/chomp_optimizer.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/MotionPlanRequest.h>

namespace chomp
{
ChompPlanner::ChompPlanner()
{
}

bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
                         moveit_msgs::MotionPlanDetailedResponse& res) const
{
  if (!planning_scene)
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
    return false;
  }

  if (req.start_state.joint_state.position.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  ros::WallTime start_time = ros::WallTime::now();
  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);

  jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
                    trajectory.getTrajectoryPoint(0));

  if (req.goal_constraints.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  if (req.goal_constraints[0].joint_constraints.empty())
  {
    ROS_ERROR_STREAM("Only joint-space goals are supported");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  int goal_index = trajectory.getNumPoints() - 1;
  trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
  sensor_msgs::JointState js;
  for (unsigned int i = 0; i < req.goal_constraints[0].joint_constraints.size(); i++)
  {
    js.name.push_back(req.goal_constraints[0].joint_constraints[i].joint_name);
    js.position.push_back(req.goal_constraints[0].joint_constraints[i].position);
    ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << req.goal_constraints[0].joint_constraints[i].joint_name
                                                            << " to position "
                                                            << req.goal_constraints[0].joint_constraints[i].position);
  }
  jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));

  const moveit::core::JointModelGroup* model_group =
      planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
  // fix the goal to move the shortest angular distance for wrap-around joints:
  for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
  {
    const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
    const moveit::core::RevoluteJointModel* revolute_joint =
        dynamic_cast<const moveit::core::RevoluteJointModel*>(model);

    if (revolute_joint != NULL)
    {
      if (revolute_joint->isContinuous())
      {
        double start = (trajectory)(0, i);
        double end = (trajectory)(goal_index, i);
        ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
        (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
      }
    }
  }

  const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
  const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
  moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
  goal_robot_state.setVariablePositions(
      active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));

  if (not goal_robot_state.satisfiesBounds())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  // fill in an initial trajectory based on user choice from the chomp_config.yaml file
  if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
    trajectory.fillInMinJerk();
  else if (params.trajectory_initialization_method_.compare("linear") == 0)
    trajectory.fillInLinearInterpolation();
  else if (params.trajectory_initialization_method_.compare("cubic") == 0)
    trajectory.fillInCubicInterpolation();
  else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
  {
    if (!(trajectory.fillInFromTrajectory(res)))
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, "
                                              "trajectory must contain at least start and goal state");
      return false;
    }
  }
  else
    ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");

  ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
                 (params.trajectory_initialization_method_).c_str());

  // optimize!
  moveit::core::RobotState start_state(planning_scene->getCurrentState());
  moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
  start_state.update();

  ros::WallTime create_time = ros::WallTime::now();

  int replan_count = 0;
  bool replan_flag = false;
  double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10;
  int org_max_iterations = 200;

  // storing the initial chomp parameters values
  org_learning_rate = params.learning_rate_;
  org_ridge_factor = params.ridge_factor_;
  org_planning_time_limit = params.planning_time_limit_;
  org_max_iterations = params.max_iterations_;

  ChompOptimizer* optimizer;

  // create a non_const_params variable which stores the non constant version of the const params variable
  ChompParameters params_nonconst = params;

  // while loop for replanning (recovery behaviour) if collision free optimized solution not found
  while (true)
  {
    if (replan_flag)
    {
      // increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5
      // additional secs in hope to find a solution; increase maximum iterations
      params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
                                        params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
    }

    // initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
    // case of a recovery behaviour
    optimizer = new ChompOptimizer(&trajectory, planning_scene, req.group_name, &params_nonconst, start_state);
    if (!optimizer->isInitialized())
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer");
      res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      return false;
    }

    ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create",
                    (ros::WallTime::now() - create_time).toSec());

    bool optimization_result = optimizer->optimize();

    // replan with updated parameters if no solution is found
    if (params_nonconst.enable_failure_recovery_)
    {
      ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, "
                                      "planning_time_limit, max_iterations), attempt: # %d ",
                     (replan_count + 1));
      ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",
                     params_nonconst.learning_rate_, params_nonconst.ridge_factor_,
                     params_nonconst.planning_time_limit_, params_nonconst.max_iterations_);

      if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_)
      {
        replan_count++;
        replan_flag = true;
        // delete ChompOptimizer object 'optimizer' to prevent a possible memory leak
        delete optimizer;
      }
      else
      {
        break;
      }
    }
    else
      break;
  }  // end of while loop

  // resetting the CHOMP Parameters to the original values after a successful plan
  params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations);

  ROS_DEBUG_NAMED("chomp_planner", "Optimization actually took %f sec to run",
                  (ros::WallTime::now() - create_time).toSec());
  create_time = ros::WallTime::now();
  // assume that the trajectory is now optimized, fill in the output structure:

  ROS_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints());

  res.trajectory.resize(1);

  res.trajectory[0].joint_trajectory.joint_names = active_joint_names;

  res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header;  // @TODO this is probably a hack

  // fill in the entire trajectory
  res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
  for (int i = 0; i < trajectory.getNumPoints(); i++)
  {
    res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
    for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
    {
      res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
    }
    // Setting invalid timestamps.
    // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
    res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
  }

  ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
  ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f",
                  (ros::WallTime::now() - start_time).toSec(),
                  res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());

  // report planning failure if path has collisions
  if (not optimizer->isCollisionFree())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // check that final state is within goal tolerances
  kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
  robot_state::RobotState last_state(start_state);
  last_state.setVariablePositions(res.trajectory[0].joint_trajectory.joint_names,
                                  res.trajectory[0].joint_trajectory.points.back().positions);

  bool constraints_are_ok = true;
  for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
  {
    constraints_are_ok = constraints_are_ok and jc.configure(constraint);
    constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
    if (not constraints_are_ok)
    {
      ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name);
      res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
      return false;
    }
  }

  return true;
}
}
<|endoftext|>


<commit_before>/*
 * sink_fixture.hpp
 *
 *  Created on: Mar 18, 2017
 *      Author: ckielwein
 */

#ifndef TESTS_PURE_SINK_FIXTURE_HPP_
#define TESTS_PURE_SINK_FIXTURE_HPP_

#include <vector>

namespace fc
{
namespace pure
{

/**
 * \brief automatically checks if certain events where received
 * \tparam T type of token accepted by sink_fixture
 */
template<class T>
class sink_fixture
{
public:
	///Initialize sink_fixture with a list of expected values
	explicit sink_fixture(std::vector<T> expected_values = std::vector<T>{})
		: expected{std::move(expected_values)}
	{
	}

	///Initialize sink_fixture with a list of expected values
	explicit sink_fixture(std::initializer_list<T> expected_values)
		: expected{expected_values}
	{
	}

	///push another value to back of the expected values
	void expect(T t)
	{
		expected.push_back(t);
	}

	///write a value to the list of received values
	void operator()(T token)
	{
		received.push_back(std::move(token));
	}

	~sink_fixture()
	{
		BOOST_CHECK_EQUAL_COLLECTIONS(
				received.begin(), received.end(),
				expected.begin(), expected.end());
	}

private:
	std::vector<T> expected;
	std::vector<T> received{};
};

} //namespace pure
} //namespace fc

#endif /* TESTS_PURE_SINK_FIXTURE_HPP_ */
<commit_msg>Initialize vector in sink_fixture with ()<commit_after>/*
 * sink_fixture.hpp
 *
 *  Created on: Mar 18, 2017
 *      Author: ckielwein
 */

#ifndef TESTS_PURE_SINK_FIXTURE_HPP_
#define TESTS_PURE_SINK_FIXTURE_HPP_

#include <vector>

namespace fc
{
namespace pure
{

/**
 * \brief automatically checks if certain events where received
 * \tparam T type of token accepted by sink_fixture
 */
template<class T>
class sink_fixture
{
public:
	///Initialize sink_fixture with a list of expected values
	explicit sink_fixture(std::vector<T> expected_values = std::vector<T>{})
		: expected(std::move(expected_values))
	{
	}

	///Initialize sink_fixture with a list of expected values
	explicit sink_fixture(std::initializer_list<T> expected_values)
		: expected(expected_values)
	{
	}

	///push another value to back of the expected values
	void expect(T t)
	{
		expected.push_back(t);
	}

	///write a value to the list of received values
	void operator()(T token)
	{
		received.push_back(std::move(token));
	}

	~sink_fixture()
	{
		BOOST_CHECK_EQUAL_COLLECTIONS(
				received.begin(), received.end(),
				expected.begin(), expected.end());
	}

private:
	std::vector<T> expected;
	std::vector<T> received{};
};

} //namespace pure
} //namespace fc

#endif /* TESTS_PURE_SINK_FIXTURE_HPP_ */
<|endoftext|>


<commit_before>#include "../kontsevich_graph_series.hpp"
#include <ginac/ginac.h>
#include <iostream>
#include <fstream>
#include <limits>
#include "../util/continued_fraction.hpp"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
using namespace std;
using namespace GiNaC;

typedef Eigen::Triplet<double> Triplet;
typedef Eigen::SparseMatrix<double> SparseMatrix;

double threshold = 1e-13;

int main(int argc, char* argv[])
{
    if (argc != 2 && argc != 3)
    {
        cout << "Usage: " << argv[0] << " <graph-series-filename> [max-jacobiators]\n\n"
             << "Accepts only homogeneous power series: graphs with n internal vertices at order n.\n"
             << "Optional argument max-jacobiators restricts the number of Jacobiators in each generated differential consequence.\n";
        return 1;
    }

    size_t max_jacobiators = numeric_limits<size_t>::max();
    if (argc == 3)
        max_jacobiators = stoi(argv[2]);

    // Reading in graph series
    string graph_series_filename(argv[1]);
    ifstream graph_series_file(graph_series_filename);
    parser coefficient_reader;
    bool homogeneous = true;
    map<size_t, set< vector<size_t> > > in_degrees;
    KontsevichGraphSeries<ex> graph_series = KontsevichGraphSeries<ex>::from_istream(graph_series_file,
        [&coefficient_reader](std::string s) -> ex { return coefficient_reader(s); },
        [&homogeneous, &in_degrees](KontsevichGraph graph, size_t order) -> bool
                                   {
                                       in_degrees[order].insert(graph.in_degrees());
                                       return homogeneous &= graph.internal() == order;
                                   }
    );
    size_t order = graph_series.precision();
    if (!homogeneous)
    {
        cerr << "Only accepting homogeneous power series: graphs with n internal vertices at order n.\n";
        return 1;
    }

    graph_series.reduce();

    size_t counter = 0;
    std::vector<symbol> coefficient_list;

    for (size_t n = 2; n <= order; ++n) // need at least 2 internal vertices for Jacobi
    {
        if (graph_series[n].size() == 0)
            continue;

        cout << "h^" << n << ":\n";

        // First we choose the target vertices i,j,k of the Jacobiators (which contain 2 bivectors), in increasing order (without loss of generality)

        // Jacobi must have three distinct arguments, and not act on itself, but can act on other Jacobi

        for (size_t k = 1; k <= min(n/2, max_jacobiators); ++k)
        {
            std::vector<size_t> jacobi_vertices(3*k, n + 3);
            CartesianProduct jacobi_indices(jacobi_vertices);
            for (auto jacobi_index = jacobi_indices.begin(); jacobi_index != jacobi_indices.end(); ++jacobi_index)
            {
                bool accept = true;
                for (size_t i = 0; i != k; ++i)
                {
                    if ((*jacobi_index)[i*3] >= (*jacobi_index)[i*3 + 1] || (*jacobi_index)[i*3 + 1] >= (*jacobi_index)[i*3 + 2]) // not strictly increasing
                    {
                        accept = false;
                        break;
                    }
                }
                if (!accept)
                    continue;

                // Then we choose the target vertices of the remaining n - 2*k bivectors, stored in a multi-index of length 2*(n-2*k)
                // Here we have k fewer possible targets: out of the last 2*k internal vertices, the first k act as placeholders for the respective Jacobiators,
                // to be replaced by the Leibniz rule later on
                std::vector<size_t> remaining_edges(2*(n-2*k), n + 3 - k);
                CartesianProduct indices(remaining_edges);
                for (auto multi_index = indices.begin(); multi_index != indices.end(); ++multi_index)
                {
                    bool accept = true;
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                    {
                        if ((*multi_index)[2*idx] >= (*multi_index)[2*idx+1])
                        {
                            accept = false; // accept only strictly increasing indices
                            break;
                        }
                        // TODO: filter out tadpoles, maybe?
                    }
                    if (!accept)
                        continue;

                    // We build the list of targets for the graph, as described above (using i,j,k and the multi-index)
                    std::vector<KontsevichGraph::VertexPair> targets(n);
                    // first part:
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                        targets[idx] = {(*multi_index)[2*idx], (*multi_index)[2*idx+1]};
                    // second part:
                    for (size_t i = 0; i != k; ++i)
                    {
                        targets[n - 2*k + 2*i].first = KontsevichGraph::Vertex((*jacobi_index)[3*i]);
                        targets[n - 2*k + 2*i].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 1]);
                        targets[n - 2*k + 2*i + 1].first = KontsevichGraph::Vertex(n + 3 - 2*k + 2*i);
                        targets[n - 2*k + 2*i + 1].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 2]);
                    }

                    // Make vector of references to bad targets: those in first part with target >= (n + 3 - 2*k), the placeholders for the Jacobiators:
                    std::map<KontsevichGraph::Vertex*, int> bad_targets;
                    for (size_t idx = 0; idx != n - 2*k; ++idx) // look for bad targets in first part
                    {
                        if ((int)targets[idx].first >= (int)n + 3 - 2*(int)k)
                            bad_targets[&targets[idx].first] = (int)targets[idx].first - (n + 3 - 2*k);
                        if ((int)targets[idx].second >= (int)n + 3 - 2*(int)k)
                            bad_targets[&targets[idx].second] = (int)targets[idx].second - (n + 3 - 2*k);
                    }

                    KontsevichGraphSum<ex> graph_sum;

                    // Replace bad targets by Leibniz rule:
                    map< vector<size_t>, symbol > coefficients;
                    std::vector<size_t> leibniz_sizes(bad_targets.size(), 2);
                    CartesianProduct leibniz_indices(leibniz_sizes);
                    for (auto leibniz_index = leibniz_indices.begin(); leibniz_index != leibniz_indices.end(); ++leibniz_index)
                    {
                        size_t idx = 0;
                        for (auto& bad_target : bad_targets)
                            *(bad_target.first) = KontsevichGraph::Vertex(3 + n - 2*k + 2*(bad_target.second) + (*leibniz_index)[idx++]);

                        for (size_t i = 0; i != k; ++i)
                        {
                            for (auto jacobi_targets_choice : std::vector< std::vector<KontsevichGraph::Vertex> >({ { targets[n-2*k+2*i].first, targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second },
                                                                                { targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first },
                                                                                { targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first, targets[n-2*k+2*i].second } }))
                            {
                                // Set Jacobiator targets to one of the three permutatations
                                targets[n-2*k+2*i].first = jacobi_targets_choice[0];
                                targets[n-2*k+2*i].second = jacobi_targets_choice[1];
                                targets[n-2*k+2*i+1].second = jacobi_targets_choice[2];

                                KontsevichGraph graph(n, 3, targets);

                                vector<size_t> indegrees = graph.in_degrees();

                                if (in_degrees[n].find(indegrees) == in_degrees[n].end()) // skip terms
                                    continue;
                                if (coefficients.find(indegrees) == coefficients.end())
                                {
                                    symbol coefficient("c_" + to_string(k) + "_" + to_string(counter) + "_" + to_string(indegrees[0]) + to_string(indegrees[1]) + to_string(indegrees[2]));
                                    coefficients[indegrees] = coefficient;
                                }
                                graph_sum += KontsevichGraphSum<ex>({ { coefficients[indegrees], graph } });
                            }
                        }
                    }

                    graph_sum.reduce();
                    if (graph_sum.size() != 0)
                    {
                        cerr << "\r" << ++counter;
                        for (auto& pair : coefficients)
                        {
                            coefficient_list.push_back(pair.second);
                        }
                    }
                    graph_series[n] -= graph_sum;
                }
            }
        }
    }

    cout << "\nNumber of coefficients: " << coefficient_list.size() << "\n";
    cout << "\nNumber of terms: " << graph_series[order].size() << "\n";
    cout << "\nNumber of terms per coefficient: " << (float)graph_series[order].size()/coefficient_list.size() << "\n";

    cout.flush();

    cerr << "\nReducing...\n";
    graph_series.reduce();

    lst equations;

    for (size_t n = 0; n <= order; ++n)
        for (auto& term : graph_series[n])
        {
            cerr << term.first << "==0\n";
            equations.append(term.first);
        }

    // Set up sparse matrix linear system

    cerr << "Setting up linear system...\n";
    size_t rows = equations.nops();
    size_t cols = coefficient_list.size();

    Eigen::VectorXd b(rows);
    SparseMatrix matrix(rows,cols);

    std::vector<Triplet> tripletList;
    size_t idx = 0;
    for (ex equation : equations)
    {
        if (!is_a<add>(equation))
            equation = lst(equation);
        for (ex term : equation)
        {
            if (!is_a<mul>(term))
                term = lst(term);
            double prefactor = 1;
            symbol coefficient("one");
            for (ex factor : term)
            {
                if (is_a<numeric>(factor))
                    prefactor *= ex_to<numeric>(factor).to_double();
                else if (is_a<symbol>(factor))
                    coefficient = ex_to<symbol>(factor);
            }
            if (coefficient.get_name() == "one") // constant term
                b(idx) = -prefactor;
            else
                tripletList.push_back(Triplet(idx,find(coefficient_list.begin(), coefficient_list.end(), coefficient) - coefficient_list.begin(), prefactor));
                // NB: Eigen uses zero-based indices (contrast MATLAB, Mathematica)
        }
        ++idx;
    }

    matrix.setFromTriplets(tripletList.begin(), tripletList.end());

    cerr << "Solving linear system numerically...\n";

    Eigen::SparseQR< SparseMatrix, Eigen::COLAMDOrdering<int> > qr(matrix);
    Eigen::VectorXd x = qr.solve(b);

    cerr << "Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Rounding...\n";
    x = x.unaryExpr([](double elem) { return fabs(elem) < threshold ? 0.0 : elem; });

    cerr << "Still a solution? Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Approximating numerical solution by rational solution...\n";

    lst zero_substitution;
    lst solution_substitution;
    for (int i = 0; i != x.size(); i++)
    {
        ex result = best_rational_approximation(x.coeff(i), threshold);
        if (result == 0)
            zero_substitution.append(coefficient_list[i] == 0);
        else
            solution_substitution.append(coefficient_list[i] == result);
    }

    cerr << "Substituting zeros...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(zero_substitution);

    cerr << "Reducing zeros...\n";

    graph_series.reduce();

    for (size_t n = 0; n <= graph_series.precision(); ++n)
    {
        cout << "h^" << n << ":\n";
        for (auto& term : graph_series[n])
        {
            cout << term.second.encoding() << "    " << term.first << "\n";
        }
    }

    cerr << "Verifying solution...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(solution_substitution);

    graph_series.reduce();

    cout << "Do we really have a solution? " << (graph_series == 0 ? "Yes" : "No") << "\n";

    for (ex subs : solution_substitution)
        cout << subs << "\n";
}
<commit_msg>Lower default threshold for approximation of floats by rationals.<commit_after>#include "../kontsevich_graph_series.hpp"
#include <ginac/ginac.h>
#include <iostream>
#include <fstream>
#include <limits>
#include "../util/continued_fraction.hpp"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
using namespace std;
using namespace GiNaC;

typedef Eigen::Triplet<double> Triplet;
typedef Eigen::SparseMatrix<double> SparseMatrix;

double threshold = 1e-5;

int main(int argc, char* argv[])
{
    if (argc != 2 && argc != 3)
    {
        cout << "Usage: " << argv[0] << " <graph-series-filename> [max-jacobiators]\n\n"
             << "Accepts only homogeneous power series: graphs with n internal vertices at order n.\n"
             << "Optional argument max-jacobiators restricts the number of Jacobiators in each generated differential consequence.\n";
        return 1;
    }

    size_t max_jacobiators = numeric_limits<size_t>::max();
    if (argc == 3)
        max_jacobiators = stoi(argv[2]);

    // Reading in graph series
    string graph_series_filename(argv[1]);
    ifstream graph_series_file(graph_series_filename);
    parser coefficient_reader;
    bool homogeneous = true;
    map<size_t, set< vector<size_t> > > in_degrees;
    KontsevichGraphSeries<ex> graph_series = KontsevichGraphSeries<ex>::from_istream(graph_series_file,
        [&coefficient_reader](std::string s) -> ex { return coefficient_reader(s); },
        [&homogeneous, &in_degrees](KontsevichGraph graph, size_t order) -> bool
                                   {
                                       in_degrees[order].insert(graph.in_degrees());
                                       return homogeneous &= graph.internal() == order;
                                   }
    );
    size_t order = graph_series.precision();
    if (!homogeneous)
    {
        cerr << "Only accepting homogeneous power series: graphs with n internal vertices at order n.\n";
        return 1;
    }

    graph_series.reduce();

    size_t counter = 0;
    std::vector<symbol> coefficient_list;

    for (size_t n = 2; n <= order; ++n) // need at least 2 internal vertices for Jacobi
    {
        if (graph_series[n].size() == 0)
            continue;

        cout << "h^" << n << ":\n";

        // First we choose the target vertices i,j,k of the Jacobiators (which contain 2 bivectors), in increasing order (without loss of generality)

        // Jacobi must have three distinct arguments, and not act on itself, but can act on other Jacobi

        for (size_t k = 1; k <= min(n/2, max_jacobiators); ++k)
        {
            std::vector<size_t> jacobi_vertices(3*k, n + 3);
            CartesianProduct jacobi_indices(jacobi_vertices);
            for (auto jacobi_index = jacobi_indices.begin(); jacobi_index != jacobi_indices.end(); ++jacobi_index)
            {
                bool accept = true;
                for (size_t i = 0; i != k; ++i)
                {
                    if ((*jacobi_index)[i*3] >= (*jacobi_index)[i*3 + 1] || (*jacobi_index)[i*3 + 1] >= (*jacobi_index)[i*3 + 2]) // not strictly increasing
                    {
                        accept = false;
                        break;
                    }
                }
                if (!accept)
                    continue;

                // Then we choose the target vertices of the remaining n - 2*k bivectors, stored in a multi-index of length 2*(n-2*k)
                // Here we have k fewer possible targets: out of the last 2*k internal vertices, the first k act as placeholders for the respective Jacobiators,
                // to be replaced by the Leibniz rule later on
                std::vector<size_t> remaining_edges(2*(n-2*k), n + 3 - k);
                CartesianProduct indices(remaining_edges);
                for (auto multi_index = indices.begin(); multi_index != indices.end(); ++multi_index)
                {
                    bool accept = true;
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                    {
                        if ((*multi_index)[2*idx] >= (*multi_index)[2*idx+1])
                        {
                            accept = false; // accept only strictly increasing indices
                            break;
                        }
                        // TODO: filter out tadpoles, maybe?
                    }
                    if (!accept)
                        continue;

                    // We build the list of targets for the graph, as described above (using i,j,k and the multi-index)
                    std::vector<KontsevichGraph::VertexPair> targets(n);
                    // first part:
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                        targets[idx] = {(*multi_index)[2*idx], (*multi_index)[2*idx+1]};
                    // second part:
                    for (size_t i = 0; i != k; ++i)
                    {
                        targets[n - 2*k + 2*i].first = KontsevichGraph::Vertex((*jacobi_index)[3*i]);
                        targets[n - 2*k + 2*i].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 1]);
                        targets[n - 2*k + 2*i + 1].first = KontsevichGraph::Vertex(n + 3 - 2*k + 2*i);
                        targets[n - 2*k + 2*i + 1].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 2]);
                    }

                    // Make vector of references to bad targets: those in first part with target >= (n + 3 - 2*k), the placeholders for the Jacobiators:
                    std::map<KontsevichGraph::Vertex*, int> bad_targets;
                    for (size_t idx = 0; idx != n - 2*k; ++idx) // look for bad targets in first part
                    {
                        if ((int)targets[idx].first >= (int)n + 3 - 2*(int)k)
                            bad_targets[&targets[idx].first] = (int)targets[idx].first - (n + 3 - 2*k);
                        if ((int)targets[idx].second >= (int)n + 3 - 2*(int)k)
                            bad_targets[&targets[idx].second] = (int)targets[idx].second - (n + 3 - 2*k);
                    }

                    KontsevichGraphSum<ex> graph_sum;

                    // Replace bad targets by Leibniz rule:
                    map< vector<size_t>, symbol > coefficients;
                    std::vector<size_t> leibniz_sizes(bad_targets.size(), 2);
                    CartesianProduct leibniz_indices(leibniz_sizes);
                    for (auto leibniz_index = leibniz_indices.begin(); leibniz_index != leibniz_indices.end(); ++leibniz_index)
                    {
                        size_t idx = 0;
                        for (auto& bad_target : bad_targets)
                            *(bad_target.first) = KontsevichGraph::Vertex(3 + n - 2*k + 2*(bad_target.second) + (*leibniz_index)[idx++]);

                        for (size_t i = 0; i != k; ++i)
                        {
                            for (auto jacobi_targets_choice : std::vector< std::vector<KontsevichGraph::Vertex> >({ { targets[n-2*k+2*i].first, targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second },
                                                                                { targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first },
                                                                                { targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first, targets[n-2*k+2*i].second } }))
                            {
                                // Set Jacobiator targets to one of the three permutatations
                                targets[n-2*k+2*i].first = jacobi_targets_choice[0];
                                targets[n-2*k+2*i].second = jacobi_targets_choice[1];
                                targets[n-2*k+2*i+1].second = jacobi_targets_choice[2];

                                KontsevichGraph graph(n, 3, targets);

                                vector<size_t> indegrees = graph.in_degrees();

                                if (in_degrees[n].find(indegrees) == in_degrees[n].end()) // skip terms
                                    continue;
                                if (coefficients.find(indegrees) == coefficients.end())
                                {
                                    symbol coefficient("c_" + to_string(k) + "_" + to_string(counter) + "_" + to_string(indegrees[0]) + to_string(indegrees[1]) + to_string(indegrees[2]));
                                    coefficients[indegrees] = coefficient;
                                }
                                graph_sum += KontsevichGraphSum<ex>({ { coefficients[indegrees], graph } });
                            }
                        }
                    }

                    graph_sum.reduce();
                    if (graph_sum.size() != 0)
                    {
                        cerr << "\r" << ++counter;
                        for (auto& pair : coefficients)
                        {
                            coefficient_list.push_back(pair.second);
                        }
                    }
                    graph_series[n] -= graph_sum;
                }
            }
        }
    }

    cout << "\nNumber of coefficients: " << coefficient_list.size() << "\n";
    cout << "\nNumber of terms: " << graph_series[order].size() << "\n";
    cout << "\nNumber of terms per coefficient: " << (float)graph_series[order].size()/coefficient_list.size() << "\n";

    cout.flush();

    cerr << "\nReducing...\n";
    graph_series.reduce();

    lst equations;

    for (size_t n = 0; n <= order; ++n)
        for (auto& term : graph_series[n])
        {
            cerr << term.first << "==0\n";
            equations.append(term.first);
        }

    // Set up sparse matrix linear system

    cerr << "Setting up linear system...\n";
    size_t rows = equations.nops();
    size_t cols = coefficient_list.size();

    Eigen::VectorXd b(rows);
    SparseMatrix matrix(rows,cols);

    std::vector<Triplet> tripletList;
    size_t idx = 0;
    for (ex equation : equations)
    {
        if (!is_a<add>(equation))
            equation = lst(equation);
        for (ex term : equation)
        {
            if (!is_a<mul>(term))
                term = lst(term);
            double prefactor = 1;
            symbol coefficient("one");
            for (ex factor : term)
            {
                if (is_a<numeric>(factor))
                    prefactor *= ex_to<numeric>(factor).to_double();
                else if (is_a<symbol>(factor))
                    coefficient = ex_to<symbol>(factor);
            }
            if (coefficient.get_name() == "one") // constant term
                b(idx) = -prefactor;
            else
                tripletList.push_back(Triplet(idx,find(coefficient_list.begin(), coefficient_list.end(), coefficient) - coefficient_list.begin(), prefactor));
                // NB: Eigen uses zero-based indices (contrast MATLAB, Mathematica)
        }
        ++idx;
    }

    matrix.setFromTriplets(tripletList.begin(), tripletList.end());

    cerr << "Solving linear system numerically...\n";

    Eigen::SparseQR< SparseMatrix, Eigen::COLAMDOrdering<int> > qr(matrix);
    Eigen::VectorXd x = qr.solve(b);

    cerr << "Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Rounding...\n";
    x = x.unaryExpr([](double elem) { return fabs(elem) < threshold ? 0.0 : elem; });

    cerr << "Still a solution? Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Approximating numerical solution by rational solution...\n";

    lst zero_substitution;
    lst solution_substitution;
    for (int i = 0; i != x.size(); i++)
    {
        ex result = best_rational_approximation(x.coeff(i), threshold);
        if (result == 0)
            zero_substitution.append(coefficient_list[i] == 0);
        else
            solution_substitution.append(coefficient_list[i] == result);
    }

    cerr << "Substituting zeros...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(zero_substitution);

    cerr << "Reducing zeros...\n";

    graph_series.reduce();

    for (size_t n = 0; n <= graph_series.precision(); ++n)
    {
        cout << "h^" << n << ":\n";
        for (auto& term : graph_series[n])
        {
            cout << term.second.encoding() << "    " << term.first << "\n";
        }
    }

    cerr << "Verifying solution...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(solution_substitution);

    graph_series.reduce();

    cout << "Do we really have a solution? " << (graph_series == 0 ? "Yes" : "No") << "\n";

    for (ex subs : solution_substitution)
        cout << subs << "\n";
}
<|endoftext|>