-
Notifications
You must be signed in to change notification settings - Fork 167
/
arm.hpp
124 lines (108 loc) · 5.48 KB
/
arm.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, University of Edinburgh, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#ifndef CROCODDYL_ARM_FACTORY_HPP_
#define CROCODDYL_ARM_FACTORY_HPP_
#include <example-robot-data/path.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/parsers/srdf.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include "crocoddyl/core/costs/cost-sum.hpp"
#include "crocoddyl/core/costs/residual.hpp"
#include "crocoddyl/core/integrator/euler.hpp"
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/residuals/control.hpp"
#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "crocoddyl/multibody/actuations/full.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/state.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"
namespace crocoddyl {
namespace benchmark {
template <typename Scalar>
void build_arm_action_models(
boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >& runningModel,
boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >&
terminalModel) {
typedef typename crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl<Scalar>
DifferentialActionModelFreeFwdDynamics;
typedef typename crocoddyl::IntegratedActionModelEulerTpl<Scalar>
IntegratedActionModelEuler;
typedef typename crocoddyl::ActuationModelFullTpl<Scalar> ActuationModelFull;
typedef typename crocoddyl::CostModelSumTpl<Scalar> CostModelSum;
typedef typename crocoddyl::CostModelAbstractTpl<Scalar> CostModelAbstract;
typedef typename crocoddyl::CostModelResidualTpl<Scalar> CostModelResidual;
typedef typename crocoddyl::ResidualModelStateTpl<Scalar> ResidualModelState;
typedef typename crocoddyl::ResidualModelFramePlacementTpl<Scalar>
ResidualModelFramePlacement;
typedef typename crocoddyl::ResidualModelControlTpl<Scalar>
ResidualModelControl;
typedef typename crocoddyl::MathBaseTpl<Scalar>::VectorXs VectorXs;
typedef typename crocoddyl::MathBaseTpl<Scalar>::Vector3s Vector3s;
typedef typename crocoddyl::MathBaseTpl<Scalar>::Matrix3s Matrix3s;
// because urdf is not supported with all scalar types.
pinocchio::ModelTpl<double> modeld;
pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR
"/talos_data/robots/talos_left_arm.urdf",
modeld);
pinocchio::srdf::loadReferenceConfigurations(
modeld, EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf",
false);
pinocchio::ModelTpl<Scalar> model_full(modeld.cast<Scalar>()), model;
std::vector<pinocchio::JointIndex> locked_joints;
locked_joints.push_back(5);
locked_joints.push_back(6);
locked_joints.push_back(7);
pinocchio::buildReducedModel(model_full, locked_joints,
VectorXs::Zero(model_full.nq), model);
boost::shared_ptr<crocoddyl::StateMultibodyTpl<Scalar> > state =
boost::make_shared<crocoddyl::StateMultibodyTpl<Scalar> >(
boost::make_shared<pinocchio::ModelTpl<Scalar> >(model));
boost::shared_ptr<CostModelAbstract> goalTrackingCost =
boost::make_shared<CostModelResidual>(
state, boost::make_shared<ResidualModelFramePlacement>(
state, model.getFrameId("gripper_left_joint"),
pinocchio::SE3Tpl<Scalar>(
Matrix3s::Identity(),
Vector3s(Scalar(0), Scalar(0), Scalar(.4)))));
boost::shared_ptr<CostModelAbstract> xRegCost =
boost::make_shared<CostModelResidual>(
state, boost::make_shared<ResidualModelState>(state));
boost::shared_ptr<CostModelAbstract> uRegCost =
boost::make_shared<CostModelResidual>(
state, boost::make_shared<ResidualModelControl>(state));
// Create a cost model per the running and terminal action model.
boost::shared_ptr<CostModelSum> runningCostModel =
boost::make_shared<CostModelSum>(state);
boost::shared_ptr<CostModelSum> terminalCostModel =
boost::make_shared<CostModelSum>(state);
// Then let's added the running and terminal cost functions
runningCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1));
runningCostModel->addCost("xReg", xRegCost, Scalar(1e-4));
runningCostModel->addCost("uReg", uRegCost, Scalar(1e-4));
terminalCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1));
// We define an actuation model
boost::shared_ptr<ActuationModelFull> actuation =
boost::make_shared<ActuationModelFull>(state);
// Next, we need to create an action model for running and terminal knots. The
// forward dynamics (computed using ABA) are implemented
// inside DifferentialActionModelFullyActuated.
boost::shared_ptr<DifferentialActionModelFreeFwdDynamics> runningDAM =
boost::make_shared<DifferentialActionModelFreeFwdDynamics>(
state, actuation, runningCostModel);
// VectorXs armature(state->get_nq());
// armature << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.;
// runningDAM->set_armature(armature);
// terminalDAM->set_armature(armature);
runningModel =
boost::make_shared<IntegratedActionModelEuler>(runningDAM, Scalar(1e-3));
terminalModel =
boost::make_shared<IntegratedActionModelEuler>(runningDAM, Scalar(0.));
}
} // namespace benchmark
} // namespace crocoddyl
#endif // CROCODDYL_ARM_FACTORY_HPP_