Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix use of actors that only has trajectory animation #1947

Merged
merged 4 commits into from
Apr 25, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions src/SdfEntityCreator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -508,6 +508,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Actor *_actor)
this->dataPtr->ecm->CreateComponent(actorEntity,
components::Name(_actor->Name()));

// Links
for (uint64_t linkIndex = 0; linkIndex < _actor->LinkCount();
++linkIndex)
{
auto link = _actor->LinkByIndex(linkIndex);
auto linkEntity = this->CreateEntities(link);

this->SetParent(linkEntity, actorEntity);
}

// Actor plugins
this->dataPtr->eventManager->Emit<events::LoadSdfPlugins>(actorEntity,
_actor->Plugins());
Expand Down
103 changes: 54 additions & 49 deletions src/rendering/RenderUtil.cc
Original file line number Diff line number Diff line change
Expand Up @@ -1204,6 +1204,13 @@ void RenderUtil::Update()
entityId, std::get<1>(model), std::get<2>(model));
}

for (const auto &actor : newActors)
{
this->dataPtr->sceneManager.CreateActor(
std::get<0>(actor), std::get<1>(actor), std::get<2>(actor),
std::get<3>(actor));
}

for (const auto &link : newLinks)
{
this->dataPtr->sceneManager.CreateLink(
Expand All @@ -1216,13 +1223,6 @@ void RenderUtil::Update()
std::get<0>(visual), std::get<1>(visual), std::get<2>(visual));
}

for (const auto &actor : newActors)
{
this->dataPtr->sceneManager.CreateActor(
std::get<0>(actor), std::get<1>(actor), std::get<2>(actor),
std::get<3>(actor));
}

for (const auto &light : newLights)
{
auto newLightRendering = this->dataPtr->sceneManager.CreateLight(
Expand Down Expand Up @@ -1336,7 +1336,7 @@ void RenderUtil::Update()
{
auto actorMesh = this->dataPtr->sceneManager.ActorMeshById(tf.first);
auto actorVisual = this->dataPtr->sceneManager.NodeById(tf.first);
if (!actorMesh || !actorVisual)
if (!actorVisual)
{
ignerr << "Actor with Entity ID '" << tf.first << "'. not found. "
<< "Skipping skeleton animation update." << std::endl;
Expand Down Expand Up @@ -1371,7 +1371,8 @@ void RenderUtil::Update()
}

tf.second.erase("actorPose");
actorMesh->SetSkeletonLocalTransforms(tf.second);
if (actorMesh)
actorMesh->SetSkeletonLocalTransforms(tf.second);
}
}
else
Expand Down Expand Up @@ -3118,7 +3119,7 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_map<Entity,
auto actorVisual = this->sceneManager.NodeById(it.first);
auto actorSkel = this->sceneManager.ActorSkeletonById(
it.first);
if (!actorMesh || !actorVisual || !actorSkel)
if (!actorVisual)
{
ignerr << "Actor with Entity ID '" << it.first << "'. not found. "
<< "Skipping skeleton animation update." << std::endl;
Expand All @@ -3131,51 +3132,55 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_map<Entity,
ignerr << "invalid animation update data" << std::endl;
continue;
}
// Enable skeleton animation
if (!actorMesh->SkeletonAnimationEnabled(animData.animationName))

if (actorMesh && actorSkel)
{
// disable all animations for this actor
for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i)
// Enable skeleton animation
if (!actorMesh->SkeletonAnimationEnabled(animData.animationName))
{
// disable all animations for this actor
for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i)
{
actorMesh->SetSkeletonAnimationEnabled(
actorSkel->Animation(i)->Name(), false, false, 0.0);
}

// enable requested animation
actorMesh->SetSkeletonAnimationEnabled(
actorSkel->Animation(i)->Name(), false, false, 0.0);
animData.animationName, true, animData.loop);

// Set skeleton root node weight to zero so it is not affected by
// the animation being played. This is needed if trajectory animation
// is enabled. We need to let the trajectory animation set the
// position of the actor instead
common::SkeletonPtr skeleton =
this->sceneManager.ActorSkeletonById(it.first);
if (skeleton)
{
float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0;
std::unordered_map<std::string, float> weights;
weights[skeleton->RootNode()->Name()] = rootBoneWeight;
actorMesh->SetSkeletonWeights(weights);
}
}

// enable requested animation
actorMesh->SetSkeletonAnimationEnabled(
animData.animationName, true, animData.loop);

// Set skeleton root node weight to zero so it is not affected by
// the animation being played. This is needed if trajectory animation
// is enabled. We need to let the trajectory animation set the
// position of the actor instead
common::SkeletonPtr skeleton =
this->sceneManager.ActorSkeletonById(it.first);
if (skeleton)
{
float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0;
std::unordered_map<std::string, float> weights;
weights[skeleton->RootNode()->Name()] = rootBoneWeight;
actorMesh->SetSkeletonWeights(weights);
// Update skeleton animation by setting animation time.
// Note that animation time is different from sim time. An actor can
// have multiple animations. Animation time is associated with
// current animation that is being played. It is also adjusted if
// interpotate_x is enabled.
actorMesh->UpdateSkeletonAnimation(animData.time);

// manually update root transform in order to sync with trajectory
// animation
if (animData.followTrajectory)
{
common::SkeletonPtr skeleton =
this->sceneManager.ActorSkeletonById(it.first);
std::map<std::string, math::Matrix4d> rootTf;
rootTf[skeleton->RootNode()->Name()] = animData.rootTransform;
actorMesh->SetSkeletonLocalTransforms(rootTf);
}
}
// Update skeleton animation by setting animation time.
// Note that animation time is different from sim time. An actor can
// have multiple animations. Animation time is associated with
// current animation that is being played. It is also adjusted if
// interpotate_x is enabled.
actorMesh->UpdateSkeletonAnimation(animData.time);

// manually update root transform in order to sync with trajectory
// animation
if (animData.followTrajectory)
{
common::SkeletonPtr skeleton =
this->sceneManager.ActorSkeletonById(it.first);
std::map<std::string, math::Matrix4d> rootTf;
rootTf[skeleton->RootNode()->Name()] = animData.rootTransform;
actorMesh->SetSkeletonLocalTransforms(rootTf);
}

// update actor trajectory animation
math::Pose3d globalPose;
Expand Down
Loading