Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	tests/main.cc	Lines:	74	77	96.1 %
Date:	2023-06-05 08:59:09	Branches:	89	192	46.4 %


/*
 *  Copyright
 */
#include <sot/pattern-generator/pg.h>

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/property_tree/ptree.hpp>
#include <iostream>
#include <iterator>
#include <pinocchio/fwd.hpp>
#include <sot/core/debug.hh>
#include <sot/core/robot-utils.hh>
#include <sstream>
#include <string>

using namespace std;

void setEndEffectorParameters(dynamicgraph::sot::RobotUtilShrPtr aRobotUtil) {
  std::string lparameter_name = "/pg/remap/r_ankle";
  std::string lparameter_value = "leg_right_6_link";
  aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);

  lparameter_name = "/pg/remap/l_ankle";
  lparameter_value = "leg_left_6_link";
  aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);

  lparameter_name = "/pg/remap/r_wrist";
  lparameter_value = "arm_right_7_link";
  aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);

  lparameter_name = "/pg/remap/l_wrist";
  lparameter_value = "arm_left_7_link";
  aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);

  lparameter_name = "/pg/remap/body";
  lparameter_value = "base_link";
  aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);

  lparameter_name = "/pg/remap/torso";
  lparameter_value = "torso_2_link";
  aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
}

void setFeetParameters(dynamicgraph::sot::RobotUtilShrPtr aRobotUtil) {
  std::vector<std::string> lparameter_names_suffix = {
      "size/height",     "size/width",      "size/depth",
      "anklePosition/x", "anklePosition/y", "anklePosition/z"};
  std::vector<double> lparameter_values = {0.122, 0.205, 0.107,
                                           0.0,   0.0,   0.107};

  std::string lparameter_names_prefix = "/robot/specificities/feet/";
  for (unsigned int i = 0; i < 6; i++) {
    std::string full_parameter_name =
        lparameter_names_prefix + "right/" + lparameter_names_suffix[i];

    aRobotUtil->set_parameter<double>(full_parameter_name,
                                      lparameter_values[i]);
    full_parameter_name =
        lparameter_names_prefix + "left/" + lparameter_names_suffix[i];
    aRobotUtil->set_parameter<double>(full_parameter_name,
                                      lparameter_values[i]);
  }
}

void setParameters(const std::string &lrobot_description) {
  dynamicgraph::sot::RobotUtilShrPtr aRobotUtil;

  // Reading the parameter.
  string model_name("robot");

  // Search for the robot util related to robot_name.
  if (!dynamicgraph::sot::isNameInRobotUtil(model_name))
    aRobotUtil = dynamicgraph::sot::createRobotUtil(model_name);

  std::string lparameter_name("/robot_description");

  // Then set the complete robot model in the parameter set.
  aRobotUtil->set_parameter<string>(lparameter_name, lrobot_description);

  /// Specify the end effectors in the parameter server object.
  setEndEffectorParameters(aRobotUtil);

  /// Specify the size of the feet.
  setFeetParameters(aRobotUtil);
}

int main(int, char **) {
  using namespace std;
  dynamicgraph::sot::PatternGenerator aPG;

  // Search talos_reduced_wpg.urdf
  string filename(EXAMPLE_ROBOT_DATA_MODEL_DIR
                  "/talos_data/robots/talos_full_v2.urdf");
  if (!boost::filesystem::exists(filename)) {
    cerr << "Unable to find talos_reduced_wpg.urdf" << endl;
    exit(-1);
  }

  // Otherwise read the file
  ifstream talos_reduced_wpg_file(filename);
  ostringstream oss;
  oss << talos_reduced_wpg_file.rdbuf();

  // Name of the parameter
  const string lparameter_name("/robot_description");

  // Model of the robot inside a string.
  const string lrobot_description = oss.str();

  std::shared_ptr<std::vector<std::string>> alist_of_robots =
      dynamicgraph::sot::getListOfRobots();

  unsigned int idx = 0;
  for (auto an_it_of_robot : *alist_of_robots) {
    std::cout << __FILE__ << " " << idx++ << " " << an_it_of_robot << std::endl;
  }

  setParameters(lrobot_description);

  // Build the pattern generator
  aPG.buildPGI();

  //  aPG.InitState();
  aPG.pgCommandLine(":samplingperiod 0.005");
  aPG.pgCommandLine(":previewcontroltime 1.6");
  aPG.pgCommandLine(":walkmode 0");
  aPG.pgCommandLine(":omega 0.0");
  aPG.pgCommandLine(":stepheight 0.05");
  aPG.pgCommandLine(":singlesupporttime 0.78");
  aPG.pgCommandLine(":doublesupporttime 0.02");
  aPG.pgCommandLine(":armparameters 0.5");
  aPG.pgCommandLine(":LimitsFeasibility 0.0");
  aPG.pgCommandLine(":ZMPShiftParameters 0.015 0.015 0.015 0.015");
  aPG.pgCommandLine(":TimeDistributeParameters 2.0 3.5 1.0 3.0");
  aPG.pgCommandLine(":UpperBodyMotionParameters -0.1 -1.0 0.0");

  dynamicgraph::Vector val_jointPosition(38);
  double val_jointPositiond[38] = {
      0.0,       0.0,       1.018213,  0.0,       0.0,       0.0,      0.0,
      0.0,       -0.411354, 0.859395,  -0.448041, -0.001708, 0.0,      0.0,
      -0.411354, 0.859395,  -0.448041, -0.001708, 0.0,       0.006761, 0.25847,
      0.173046,  -0.0002,   -0.525366, 0.0,       -0.0,      0.1,      -0.005,
      -0.25847,  -0.173046, 0.0002,    -0.525366, 0.0,       0.0,      0.1,
      -0.005,    0.,        0.};

  for (unsigned int i = 0; i < val_jointPosition.size(); i++)
    val_jointPosition[i] = val_jointPositiond[i];
  aPG.jointPositionSIN.setConstant(val_jointPosition);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright
3			*/
4			#include <sot/pattern-generator/pg.h>
5
6			#include <algorithm>
7			#include <boost/algorithm/string.hpp>
8			#include <boost/filesystem.hpp>
9			#include <boost/property_tree/ptree.hpp>
10			#include <iostream>
11			#include <iterator>
12			#include <pinocchio/fwd.hpp>
13			#include <sot/core/debug.hh>
14			#include <sot/core/robot-utils.hh>
15			#include <sstream>
16			#include <string>
17
18			using namespace std;
19
20		1	void setEndEffectorParameters(dynamicgraph::sot::RobotUtilShrPtr aRobotUtil) {
21	✓✗	2	std::string lparameter_name = "/pg/remap/r_ankle";
22	✓✗	2	std::string lparameter_value = "leg_right_6_link";
23	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
24
25	✓✗	1	lparameter_name = "/pg/remap/l_ankle";
26	✓✗	1	lparameter_value = "leg_left_6_link";
27	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
28
29	✓✗	1	lparameter_name = "/pg/remap/r_wrist";
30	✓✗	1	lparameter_value = "arm_right_7_link";
31	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
32
33	✓✗	1	lparameter_name = "/pg/remap/l_wrist";
34	✓✗	1	lparameter_value = "arm_left_7_link";
35	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
36
37	✓✗	1	lparameter_name = "/pg/remap/body";
38	✓✗	1	lparameter_value = "base_link";
39	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
40
41	✓✗	1	lparameter_name = "/pg/remap/torso";
42	✓✗	1	lparameter_value = "torso_2_link";
43	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lparameter_value);
44		1	}
45
46		1	void setFeetParameters(dynamicgraph::sot::RobotUtilShrPtr aRobotUtil) {
47			std::vector<std::string> lparameter_names_suffix = {
48			"size/height", "size/width", "size/depth",
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	10	"anklePosition/x", "anklePosition/y", "anklePosition/z"};
50			std::vector<double> lparameter_values = {0.122, 0.205, 0.107,
51	✓✗	2	0.0, 0.0, 0.107};
52
53	✓✗	2	std::string lparameter_names_prefix = "/robot/specificities/feet/";
54	✓✓	7	for (unsigned int i = 0; i < 6; i++) {
55			std::string full_parameter_name =
56	✓✗✓✗	12	lparameter_names_prefix + "right/" + lparameter_names_suffix[i];
57
58	✓✗	12	aRobotUtil->set_parameter<double>(full_parameter_name,
59		6	lparameter_values[i]);
60			full_parameter_name =
61	✓✗✓✗	6	lparameter_names_prefix + "left/" + lparameter_names_suffix[i];
62	✓✗	12	aRobotUtil->set_parameter<double>(full_parameter_name,
63		6	lparameter_values[i]);
64			}
65		1	}
66
67		1	void setParameters(const std::string &lrobot_description) {
68		1	dynamicgraph::sot::RobotUtilShrPtr aRobotUtil;
69
70			// Reading the parameter.
71	✓✗	2	string model_name("robot");
72
73			// Search for the robot util related to robot_name.
74	✓✗✓✗	1	if (!dynamicgraph::sot::isNameInRobotUtil(model_name))
75	✓✗	1	aRobotUtil = dynamicgraph::sot::createRobotUtil(model_name);
76
77	✓✗	1	std::string lparameter_name("/robot_description");
78
79			// Then set the complete robot model in the parameter set.
80	✓✗	1	aRobotUtil->set_parameter<string>(lparameter_name, lrobot_description);
81
82			/// Specify the end effectors in the parameter server object.
83	✓✗	1	setEndEffectorParameters(aRobotUtil);
84
85			/// Specify the size of the feet.
86	✓✗	1	setFeetParameters(aRobotUtil);
87		1	}
88
89		1	int main(int, char **) {
90			using namespace std;
91	✓✗✓✗	3	dynamicgraph::sot::PatternGenerator aPG;
92
93			// Search talos_reduced_wpg.urdf
94			string filename(EXAMPLE_ROBOT_DATA_MODEL_DIR
95	✓✗	2	"/talos_data/robots/talos_full_v2.urdf");
96	✓✗✓✗ ✗✓	1	if (!boost::filesystem::exists(filename)) {
97			cerr << "Unable to find talos_reduced_wpg.urdf" << endl;
98			exit(-1);
99			}
100
101			// Otherwise read the file
102	✓✗	2	ifstream talos_reduced_wpg_file(filename);
103	✓✗	2	ostringstream oss;
104	✓✗✓✗	1	oss << talos_reduced_wpg_file.rdbuf();
105
106			// Name of the parameter
107	✓✗	2	const string lparameter_name("/robot_description");
108
109			// Model of the robot inside a string.
110	✓✗	2	const string lrobot_description = oss.str();
111
112			std::shared_ptr<std::vector<std::string>> alist_of_robots =
113	✓✗	2	dynamicgraph::sot::getListOfRobots();
114
115		1	unsigned int idx = 0;
116	✗✓✗✗	1	for (auto an_it_of_robot : *alist_of_robots) {
117			std::cout << __FILE__ << " " << idx++ << " " << an_it_of_robot << std::endl;
118			}
119
120	✓✗	1	setParameters(lrobot_description);
121
122			// Build the pattern generator
123	✓✗	1	aPG.buildPGI();
124
125			// aPG.InitState();
126	✓✗✓✗	1	aPG.pgCommandLine(":samplingperiod 0.005");
127	✓✗✓✗	1	aPG.pgCommandLine(":previewcontroltime 1.6");
128	✓✗✓✗	1	aPG.pgCommandLine(":walkmode 0");
129	✓✗✓✗	1	aPG.pgCommandLine(":omega 0.0");
130	✓✗✓✗	1	aPG.pgCommandLine(":stepheight 0.05");
131	✓✗✓✗	1	aPG.pgCommandLine(":singlesupporttime 0.78");
132	✓✗✓✗	1	aPG.pgCommandLine(":doublesupporttime 0.02");
133	✓✗✓✗	1	aPG.pgCommandLine(":armparameters 0.5");
134	✓✗✓✗	1	aPG.pgCommandLine(":LimitsFeasibility 0.0");
135	✓✗✓✗	1	aPG.pgCommandLine(":ZMPShiftParameters 0.015 0.015 0.015 0.015");
136	✓✗✓✗	1	aPG.pgCommandLine(":TimeDistributeParameters 2.0 3.5 1.0 3.0");
137	✓✗✓✗	1	aPG.pgCommandLine(":UpperBodyMotionParameters -0.1 -1.0 0.0");
138
139	✓✗	2	dynamicgraph::Vector val_jointPosition(38);
140		1	double val_jointPositiond[38] = {
141			0.0, 0.0, 1.018213, 0.0, 0.0, 0.0, 0.0,
142			0.0, -0.411354, 0.859395, -0.448041, -0.001708, 0.0, 0.0,
143			-0.411354, 0.859395, -0.448041, -0.001708, 0.0, 0.006761, 0.25847,
144			0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005,
145			-0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1,
146			-0.005, 0., 0.};
147
148	✓✗✓✓	39	for (unsigned int i = 0; i < val_jointPosition.size(); i++)
149	✓✗	38	val_jointPosition[i] = val_jointPositiond[i];
150	✓✗	1	aPG.jointPositionSIN.setConstant(val_jointPosition);
151		1	}