기록하자
꾸준히 노력하고 있습니다.

1 분 소요

Intro

이번 프로젝트에서는 인간 모델 모션(walking, running, sitdown, standup)을 구현해보려고 한다.

로봇 시뮬레이션에서 모션을 가진 actor 모델들에 대한 필요성은 다음과 같다.

  • 환경 상호작용 시뮬레이션:

    인간 모델은 로봇이 실제 환경에서 상호작용하는 방식을 시뮬레이션하는 데 중요하다. 인간의 동작은 주변 환경에 영향을 줄 수 있으며, 이를 고려하여 로봇 시스템을 설계하고 테스트하는 것이 중요하다.

  • 작업 환경 모델링:

    로봇이 협업하거나 인간과 공동 작업하는 시나리오를 모델링할 때 인간 모델은 필수적이다. 인간의 동작은 로봇의 동작을 결정하거나 보조할 수 있으며, 이를 통해 작업 효율성과 안전성을 향상시킬 수 있다.

  • 안전 및 충돌 검사:

    인간 모델은 로봇이 작동하는 동안 발생할 수 있는 충돌이나 안전 문제를 검사하는 데 사용될 수 있다. 로봇의 동작이 인간과 충돌하거나 인간의 안전에 위험을 초래할 수 있는 경우를 사전에 파악하여 수정할 수 있다.

  • 사용자 경험 시뮬레이션:

    로봇 시스템이 사용자와 상호작용하는 경우, 인간 모델은 사용자 경험을 시뮬레이션하는 데 사용될 수 있다. 로봇이 예상되는 환경에서 어떻게 동작할지 미리 확인함으로써 시스템의 사용자 친화성을 향상시킬 수 있다.

따라서 로봇 시뮬레이션에서 인간 모델이 다양한 측면에서 중요한 역할을 한다.

1. OpenSource

GAZEBO 오픈소스를 통해 장애물 회피 기능을 가진 walking 모델을 확인할 수 있다.

https://github.com/gazebosim/gazebo-classic/tree/gazebo11/plugins

ActorPlugin.cc
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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
/*
 * Copyright (C) 2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
 
#include <functional>
 
#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "plugins/ActorPlugin.hh"
 
using namespace gazebo;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin)
 
#define WALKING_ANIMATION "walking"
 
/////////////////////////////////////////////////
ActorPlugin::ActorPlugin()
{
}
 
/////////////////////////////////////////////////
void ActorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  this->sdf = _sdf;
  this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
  this->world = this->actor->GetWorld();
 
  this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
          std::bind(&ActorPlugin::OnUpdate, this, std::placeholders::_1)));
 
  this->Reset();
 
  // Read in the target weight
  if (_sdf->HasElement("target_weight"))
    this->targetWeight = _sdf->Get<double>("target_weight");
  else
    this->targetWeight = 1.15;
 
  // Read in the obstacle weight
  if (_sdf->HasElement("obstacle_weight"))
    this->obstacleWeight = _sdf->Get<double>("obstacle_weight");
  else
    this->obstacleWeight = 1.5;
 
  // Read in the animation factor (applied in the OnUpdate function).
  if (_sdf->HasElement("animation_factor"))
    this->animationFactor = _sdf->Get<double>("animation_factor");
  else
    this->animationFactor = 4.5;
 
  // Add our own name to models we should ignore when avoiding obstacles.
  this->ignoreModels.push_back(this->actor->GetName());
 
  // Read in the other obstacles to ignore
  if (_sdf->HasElement("ignore_obstacles"))
  {
    sdf::ElementPtr modelElem =
      _sdf->GetElement("ignore_obstacles")->GetElement("model");
    while (modelElem)
    {
      this->ignoreModels.push_back(modelElem->Get<std::string>());
      modelElem = modelElem->GetNextElement("model");
    }
  }
}
 
/////////////////////////////////////////////////
void ActorPlugin::Reset()
{
  this->velocity = 0.8;
  this->lastUpdate = 0;
 
  if (this->sdf && this->sdf->HasElement("target"))
    this->target = this->sdf->Get<ignition::math::Vector3d>("target");
  else
    this->target = ignition::math::Vector3d(0, -5, 1.2138);
 
  auto skelAnims = this->actor->SkeletonAnimations();
  if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
  {
    gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
  }
  else
  {
    // Create custom trajectory
    this->trajectoryInfo.reset(new physics::TrajectoryInfo());
    this->trajectoryInfo->type = WALKING_ANIMATION;
    this->trajectoryInfo->duration = 1.0;
 
    this->actor->SetCustomTrajectory(this->trajectoryInfo);
  }
}
 
/////////////////////////////////////////////////
void ActorPlugin::ChooseNewTarget()
{
  ignition::math::Vector3d newTarget(this->target);
  while ((newTarget - this->target).Length() < 2.0)
  {
    newTarget.X(ignition::math::Rand::DblUniform(-3, 3.5));
    newTarget.Y(ignition::math::Rand::DblUniform(-10, 2));
 
    for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
    {
      double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
          - newTarget).Length();
      if (dist < 2.0)
      {
        newTarget = this->target;
        break;
      }
    }
  }
  this->target = newTarget;
}
 
/////////////////////////////////////////////////
void ActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
{
  for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
  {
    physics::ModelPtr model = this->world->ModelByIndex(i);
    if (std::find(this->ignoreModels.begin(), this->ignoreModels.end(),
          model->GetName()) == this->ignoreModels.end())
    {
      ignition::math::Vector3d offset = model->WorldPose().Pos() -
        this->actor->WorldPose().Pos();
      double modelDist = offset.Length();
      if (modelDist < 4.0)
      {
        double invModelDist = this->obstacleWeight / modelDist;
        offset.Normalize();
        offset *= invModelDist;
        _pos -= offset;
      }
    }
  }
}
 
/////////////////////////////////////////////////
void ActorPlugin::OnUpdate(const common::UpdateInfo &_info)
{
  // Time delta
  double dt = (_info.simTime - this->lastUpdate).Double();
 
  ignition::math::Pose3d pose = this->actor->WorldPose();
  ignition::math::Vector3d pos = this->target - pose.Pos();
  ignition::math::Vector3d rpy = pose.Rot().Euler();
 
  double distance = pos.Length();
 
  // Choose a new target position if the actor has reached its current
  // target.
  if (distance < 0.3)
  {
    this->ChooseNewTarget();
    pos = this->target - pose.Pos();
  }
 
  // Normalize the direction vector, and apply the target weight
  pos = pos.Normalize() * this->targetWeight;
 
  // Adjust the direction vector by avoiding obstacles
  this->HandleObstacles(pos);
 
  // Compute the yaw orientation
  ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
  yaw.Normalize();
 
  // Rotate in place, instead of jumping.
  if (std::abs(yaw.Radian()) > IGN_DTOR(10))
  {
    pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+
        yaw.Radian()*0.001);
  }
  else
  {
    pose.Pos() += pos * this->velocity * dt;
    pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
  }
 
  // Make sure the actor stays within bounds
  pose.Pos().X(std::max(-3.0, std::min(3.5, pose.Pos().X())));
  pose.Pos().Y(std::max(-10.0, std::min(2.0, pose.Pos().Y())));
  pose.Pos().Z(1.2138);
 
  // Distance traveled is used to coordinate motion with the walking
  // animation
  double distanceTraveled = (pose.Pos() -
      this->actor->WorldPose().Pos()).Length();
 
  this->actor->SetWorldPose(pose, false, false);
  this->actor->SetScriptTime(this->actor->ScriptTime() +
    (distanceTraveled * this->animationFactor));
  this->lastUpdate = _info.simTime;
}
 
cs
ActorPlugin.hh
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
/*
 * Copyright (C) 2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
 
#ifndef GAZEBO_PLUGINS_ACTORPLUGIN_HH_
#define GAZEBO_PLUGINS_ACTORPLUGIN_HH_
 
#include <string>
#include <vector>
 
#include "gazebo/common/Plugin.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/util/system.hh"
 
namespace gazebo
{
  class GZ_PLUGIN_VISIBLE ActorPlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: ActorPlugin();
 
    /// \brief Load the actor plugin.
    /// \param[in] _model Pointer to the parent model.
    /// \param[in] _sdf Pointer to the plugin's SDF elements.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
 
    // Documentation Inherited.
    public: virtual void Reset();
 
    /// \brief Function that is called every update cycle.
    /// \param[in] _info Timing information
    private: void OnUpdate(const common::UpdateInfo &_info);
 
    /// \brief Helper function to choose a new target location
    private: void ChooseNewTarget();
 
    /// \brief Helper function to avoid obstacles. This implements a very
    /// simple vector-field algorithm.
    /// \param[in] _pos Direction vector that should be adjusted according
    /// to nearby obstacles.
    private: void HandleObstacles(ignition::math::Vector3d &_pos);
 
    /// \brief Pointer to the parent actor.
    private: physics::ActorPtr actor;
 
    /// \brief Pointer to the world, for convenience.
    private: physics::WorldPtr world;
 
    /// \brief Pointer to the sdf element.
    private: sdf::ElementPtr sdf;
 
    /// \brief Velocity of the actor
    private: ignition::math::Vector3d velocity;
 
    /// \brief List of connections
    private: std::vector<event::ConnectionPtr> connections;
 
    /// \brief Current target location
    private: ignition::math::Vector3d target;
 
    /// \brief Target location weight (used for vector field)
    private: double targetWeight = 1.0;
 
    /// \brief Obstacle weight (used for vector field)
    private: double obstacleWeight = 1.0;
 
    /// \brief Time scaling factor. Used to coordinate translational motion
    /// with the actor's walking animation.
    private: double animationFactor = 1.0;
 
    /// \brief Time of the last update.
    private: common::Time lastUpdate;
 
    /// \brief List of models to ignore. Used for vector field
    private: std::vector<std::string> ignoreModels;
 
    /// \brief Custom trajectory info.
    private: physics::TrajectoryInfoPtr trajectoryInfo;
  };
}
#endif
 
 
cs

호출 구조를 간단하게 파악한 바는 아래와 같다.

Untitled Diagram drawio(1)

1.1. 문제점

해당 오픈소스의 경우 'walking' 모션만 적용되어 있기 때문에 추가적으로 인간의 모션을 파악하는데 있어서 제한되는 부분이 많고, 다양한 모션을 가진 환경구성에 어려움이 있다.

1.2. 해결방안

'void ActorPlugin::ChooseNewTarget()' 함수 호출 횟수에 대한 주기를 임의로 할당하여 해당되는 NUM 만큼 호출되었을 때 'ChangeMotion_Ex' 함수를 호출하여 모션을 변경할 수 있도록 해야겠다.

Untitled Diagram drawio(2)

References

참고

[ROS2] Hello World

3 분 소요

Intro C++ 언어로 간단한 구조의 토픽(Topic) 퍼블리셔(Publisher)와 서브스크라이버(Subscriber)를 작성하고 동작시켜보자.

[ROS2] 빌드 설정 파일(CMakeLists.txt)

최대 1 분 소요

CMakeLists.txt 파일이란? ROS2의 빌드 시스템은 ament에서는 C++ 프로그래밍 언어를 사용한 패키지나 RQt Plugin의 경우 CMake (Cross Platform Make)를 이용하고 있고 패키지 폴더의 CMakeLists.txt 파일에 빌드 환경을 기술해 ...