Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
  • Loading branch information
@markaren
markaren committed Jun 25, 2024
1 parent 75f0e60 commit ae31aa6
Show file tree
Hide file tree
Showing 3 changed files with 31 additions and 29 deletions.
2 changes: 1 addition & 1 deletion examples/loaders/urdf_loader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ int main(int argc, char** argv) {
camera->position.set(0, size.y * 1.5f, size.z * 3.f);
controls.update();

bool animate{true};
bool animate{false};
bool showColliders{false};
const auto info = robot->getArticulatedJointInfo();
std::vector<float> jointValues = robot->jointValuesWithConversionFromRadiansToDeg();
Expand Down
15 changes: 11 additions & 4 deletions include/threepp/objects/Robot.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,9 @@ namespace threepp {
joints_.emplace_back(joint);
jointInfos_.emplace_back(info);
if (info.type != JointType::Fixed) {
articulatedJoints_.emplace(joints_.size() - 1, std::make_pair(joint.get(), info));
size_t idx = joints_.size() -1;
articulatedJoints_.emplace(idx, std::make_pair(joint.get(), info));
origPose_.emplace(idx, std::make_pair(joint->position.clone(), joint->quaternion.clone()));
}
}

Expand Down Expand Up @@ -87,16 +89,19 @@ namespace threepp {
jointValues_.resize(numDOF());
}

void setJointValues(std::vector<float> values) {
void setJointValues(const std::vector<float>& values) {
for (auto i = 0; i < values.size(); ++i) {
setJointValue(i, values[i]);
}
}

void setJointValue(int index, float value, bool deg = false) {

const auto& joint = articulatedJoints_[index].first;
const auto& info = articulatedJoints_[index].second;
const auto& joint = articulatedJoints_.at(index).first;
const auto& info = articulatedJoints_.at(index).second;

const auto& origPos = origPose_.at(index).first;
const auto& origQuat = origPose_.at(index).second;

switch (info.type) {
case JointType::Revolute: {
Expand All @@ -105,6 +110,7 @@ namespace threepp {
value = info.range->clamp(value);
}
joint->quaternion.setFromAxisAngle(info.axis, value);
joint->quaternion.premultiply(origQuat);
jointValues_[index] = value;
break;
}
Expand Down Expand Up @@ -179,6 +185,7 @@ namespace threepp {
std::vector<std::shared_ptr<Object3D>> colliders_;
std::vector<std::shared_ptr<Object3D>> joints_;
std::unordered_map<size_t, std::pair<Object3D*, JointInfo>> articulatedJoints_;
std::unordered_map<size_t, std::pair<Vector3, Quaternion>> origPose_;

std::vector<float> jointValues_;
};
Expand Down
43 changes: 19 additions & 24 deletions src/threepp/loaders/URDFLoader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,7 @@ namespace {
const auto fileName = getModelPath(path.parent_path(), mesh.attribute("filename").value());

if (auto obj = loader.load(fileName)) {

obj->traverseType<Mesh>([fileName](const Mesh& mesh) {
if (fileName.extension().string() == ".stl" || fileName.extension().string() == ".STL") {
mesh.geometry()->applyMatrix4(Matrix4().makeRotationX(-math::PI / 2));
Expand All @@ -161,14 +162,14 @@ namespace {
return obj;

} else if (const auto sphere = geometry.child("sphere")) {
const auto radius = utils::parseFloat(box.attribute("radius").value());
const auto radius = utils::parseFloat(sphere.attribute("radius").value());
auto obj = Mesh::create(SphereGeometry::create(radius));

return obj;

} else if (const auto cylinder = geometry.child("cylinder")) {
const auto radius = utils::parseFloat(box.attribute("radius").value());
const auto length = utils::parseFloat(box.attribute("length").value());
const auto radius = utils::parseFloat(cylinder.attribute("radius").value());
const auto length = utils::parseFloat(cylinder.attribute("length").value());
auto obj = Mesh::create(CylinderGeometry::create(radius, radius, length));

return obj;
Expand Down Expand Up @@ -199,32 +200,29 @@ struct URDFLoader::Impl {
const auto linkObject = std::make_shared<Object3D>();
linkObject->name = link.attribute("name").value();

if (const auto visual = link.child("visual")) {
for (const auto visual : link.children("visual")) {

auto group = Group::create();
group->position.copy(getXYZ(visual));
applyRotation(group, getRPY(visual));

if (const auto geometry = visual.child("geometry")) {

if (auto visualObject = parseGeometryNode(path, loader, geometry)) {
group->add(visualObject);
}
if (auto visualObject = parseGeometryNode(path, loader, visual.child("geometry"))) {
group->add(visualObject);
}

if (const auto material = visual.child("material")) {
if (const auto material = visual.child("material")) {

const auto mtl = getMaterial(material);
const auto mtl = getMaterial(material);

group->traverseType<Mesh>([mtl](Mesh& mesh) {
mesh.setMaterial(mtl);
});
}
group->traverseType<Mesh>([mtl](Mesh& mesh) {
mesh.setMaterial(mtl);
});
}

linkObject->add(group);
}

if (const auto collider = link.child("collision")) {
for (const auto collider : link.children("collision")) {

auto group = Group::create();
group->userData["collider"] = true;
Expand All @@ -234,15 +232,12 @@ struct URDFLoader::Impl {
const auto material = MeshBasicMaterial::create();
material->wireframe = true;

if (const auto geometry = collider.child("geometry")) {

if (auto colliderObject = parseGeometryNode(path, loader, geometry)) {
group->add(colliderObject);
if (auto colliderObject = parseGeometryNode(path, loader, collider.child("geometry"))) {
group->add(colliderObject);

colliderObject->traverseType<Mesh>([material](Mesh& mesh) {
mesh.setMaterial(material);
});
}
colliderObject->traverseType<Mesh>([material](Mesh& mesh) {
mesh.setMaterial(material);
});
}

linkObject->add(group);
Expand Down

0 comments on commit ae31aa6

Please sign in to comment.