[PROJECTS] Pedsim_Motion

February 7, 2024 2 분 소요

Pedsim_Motion

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

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

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

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

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

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

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

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

1.2. 해결방안

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

Untitled Diagram drawio(2)

2. 목표

구현하고자 하는 'actor'의 모션은 다음과 같다.

Walking & SitDown & StandUp
Running
Walking & Running

현재 gazebo에서 제공하는 오픈소스를 이용해서 위의 모션을 구현할 생각이다.

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;
}
 
Colored by Color Scripter
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
 
 
Colored by Color Scripter
cs

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

Untitled Diagram drawio(1)

3. 과정

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

따라서 gazebo에서 제공하는 여러가지 motion DAE 파일들을 이용하여 여러가지 모션을 적절히 추가하는 방식으로 진행할 것이다.

가장 까다로운 모션은 'Walking & SitDown & SitUp' 모션이기 때문에 해당 모션을 구현하면 다른 모션들은 상대적으로 수월하게 생성할 수 있을 것 같다.

3.1. Motion 주기성 부여

'actor'가 걷던 도중에 앉았다 일어나는 모션의 주기성을 부여하는 것이 중요하다.

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

Untitled Diagram drawio(2)

3.2. SitDown & StandUp 모션 생성

'actor'가 일정한 주기를 가지고 앉았다 일어나는 데 걸리는 시간 변수를 class 변수에 선언을 해주고 'SitDown', 'Sit', 'StandUp' 모션 각각을 추가해 주었다.

    // SitDown Motion : 2.4 sec
    // StnadUp Motion : 2 sec
  ...

  else
  {
    if (this->cycleTime_Sitting_Walk <= SittingTime)
    {
      //this->trajectoryInfo.reset(new physics::TrajectoryInfo());

      if (this->cycleTime_Sitting_Walk < 2.4)
      {
        this->trajectoryInfo->type = "sitdown";
        pose = this->actor->WorldPose();
        pose.Pos().Z(pose.Pos().Z() - 0.0002);
        pose.Rot() = ignition::math::Quaterniond(rpy.X() - 0.0002, rpy.Y(), rpy.Z());
        this->actor->SetWorldPose(pose);   
      }
      else if (this->cycleTime_Sitting_Walk <= (SittingTime - 2))
      {
        this->actor->SetWorldPose(pose);
        this->trajectoryInfo->type = "sitting";
      }
      else
      {
        this->trajectoryInfo->type = "standup";
        pose = this->actor->WorldPose();
        pose.Pos().Z(pose.Pos().Z() + 0.0002);
        pose.Rot() = ignition::math::Quaterniond(rpy.X() + 0.0002, rpy.Y(), rpy.Z());
        this->actor->SetWorldPose(pose); 
      }

    

      this->trajectoryInfo->duration = 100.0;
      
      this->actor->SetScriptTime(this->actor->ScriptTime() +
        ((dt * 0.5) * this->animationFactor));
      this->lastUpdate = _info.simTime;   
      this->cycleTime_Sitting_Walk += dt;
    }
    else
    {
      this->CountCall = 0;
      this->cycleTime_Sitting_Walk = 0.0;
      this->trajectoryInfo.reset(new physics::TrajectoryInfo());
      this->trajectoryInfo->type = WALKING_ANIMATION;
      this->actor->SetCustomTrajectory(this->trajectoryInfo);
    }
  }

4. GitHub

구현한 모션들에 대한 디렉터리 및 파일은 GitHub에 업로드하였다.

https://github.com/Juunghyeon/pedsim_motion

References

Twitter Facebook LinkedIn

Kim Jyeonghyeon

[PROJECTS] Pedsim_Motion

Pedsim_Motion

1.2. 해결방안

2. 목표

3. 과정

3.1. Motion 주기성 부여

3.2. SitDown & StandUp 모션 생성

4. GitHub

References

공유하기

참고

[ROS2] Issues

[ROS2] Hello World

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

[ROS2] Modeling Packages