task-actuation-bounds.cpp 2.8 KB
Newer Older
1
2
3
//
// Copyright (c) 2017 CNRS
//
jcarpent's avatar
jcarpent committed
4
5
// This file is part of tsid
// tsid is free software: you can redistribute it
6
7
8
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
jcarpent's avatar
jcarpent committed
9
// tsid is distributed in the hope that it will be
10
11
12
13
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
jcarpent's avatar
jcarpent committed
14
// tsid If not, see
15
16
17
// <http://www.gnu.org/licenses/>.
//

18
19
20
#include <tsid/tasks/task-actuation-bounds.hpp>
#include "tsid/robots/robot-wrapper.hpp"

jcarpent's avatar
jcarpent committed
21
namespace tsid
22
23
24
{
  namespace tasks
  {
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
    using namespace math;
    using namespace trajectories;
    using namespace pinocchio;

    TaskActuationBounds::TaskActuationBounds(const std::string & name,
                                             RobotWrapper & robot):
      TaskActuation(name, robot),
      m_constraint(name, robot.na(), robot.na())
    {
      Vector m = Vector::Ones(robot.na());
      mask(m);
    }

    const Vector & TaskActuationBounds::mask() const
    {
      return m_mask;
    }

    void TaskActuationBounds::mask(const Vector & m)
    {
      assert(m.size()==m_robot.na());
      m_mask = m;
      const Vector::Index dim = static_cast<Vector::Index>(m.sum());
      Matrix S = Matrix::Zero(dim, m_robot.na());
      m_activeAxes.resize(dim);
      unsigned int j=0;
      for(unsigned int i=0; i<m.size(); i++)
        if(m(i)!=0.0)
        {
          assert(m(i)==1.0);
          S(j,i) = 1.0;
          m_activeAxes(j) = i;
          j++;
        }
      m_constraint.resize((unsigned int)dim, m_robot.na());
      m_constraint.setMatrix(S);
    }

    int TaskActuationBounds::dim() const
    {
      return (int)m_mask.sum();
    }

    const Vector & TaskActuationBounds::getLowerBounds() const { return m_constraint.lowerBound(); }

    const Vector & TaskActuationBounds::getUpperBounds() const { return m_constraint.upperBound(); }

    void TaskActuationBounds::setBounds(ConstRefVector lower, ConstRefVector upper)
    {
      assert(lower.size()==dim());
      assert(upper.size()==dim());
      m_constraint.setLowerBound(lower);
      m_constraint.setUpperBound(upper);
    }

    const ConstraintBase & TaskActuationBounds::getConstraint() const
    {
      return m_constraint;
    }

    const ConstraintBase & TaskActuationBounds::compute(const double ,
                                                        ConstRefVector ,
                                                        ConstRefVector ,
                                                        const Data & )
    {
      return m_constraint;
    }

93
94
  }
}