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: update tutorial doc to reflect new APIs #1481

Merged
merged 7 commits into from
Aug 16, 2020
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
12 changes: 6 additions & 6 deletions docs/readthedocs/tutorials/biped.md
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ SkeletonPtr loadBiped()
{
...
for(size_t i = 0; i < biped->getNumJoints(); ++i)
biped->getJoint(i)->setPositionLimited(true);
biped->getJoint(i)->setLimitEnforcement(true);
...
}
```
Expand All @@ -75,15 +75,15 @@ a skeleton. You can enable self-collision checking on the biped by
SkeletonPtr loadBiped()
{
...
biped->enableSelfCollision();
biped->enableSelfCollisionCheck();
...
}
```
This function will enable self-collision on every non-adjacent pair of
body nodes. If you wish to also enable self-collision on adjacent body
nodes, set the optional parameter to true:
This function will enable self-collision on every pair of
body nodes. If you wish to disable self-collisions on adjacent body
nodes, call the following function
```cpp
biped->enableSelfCollision(true);
biped->disableAdjacentBodyCheck();
```
Running the program again, you should see that the character is still
floppy like a ragdoll, but now the joints do not bend backward and the
Expand Down
91 changes: 41 additions & 50 deletions docs/readthedocs/tutorials/collisions.md
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ consistent API that works cleanly for fundamentally different types of classes.

To create a ``Properties`` class for our Joint type, we'll want to say
```cpp
typename JointType::Properties properties;
typename JointType::Properties joint_properties;
```

We need to include the ``typename`` keywords because of how the syntax works for
Expand All @@ -67,7 +67,7 @@ wants you to be aware when you are being negligent about naming things). For the
sake of simplicity, let's just give it a name based off its child BodyNode:

```cpp
properties.mName = name+"_joint";
joint_properties.mName = name+"_joint";
```

Don't forget to uncomment the function arguments.
Expand Down Expand Up @@ -105,8 +105,8 @@ We can then offset the parent and child BodyNodes of this Joint using this
transform:

```cpp
properties.mT_ParentBodyToJoint = tf;
properties.mT_ChildBodyToJoint = tf.inverse();
joint_properties.mT_ParentBodyToJoint = tf;
joint_properties.mT_ChildBodyToJoint = tf.inverse();
```

Remember that all of that code should go inside the ``if(parent)`` condition.
Expand All @@ -127,7 +127,7 @@ that new BodyNode:

```cpp
BodyNode* bn = chain->createJointAndBodyNodePair<JointType>(
parent, properties, BodyNode::AspectProperties(name)).second;
parent, joint_properties, BodyNode::AspectProperties(name)).second;
```

There's a lot going on in this function, so let's break it down for a moment:
Expand All @@ -145,13 +145,13 @@ type that we want to create, but the default argument is a standard rigid
BodyNode, so we can leave the second argument blank.

```cpp
(parent, properties, BodyNode::AspectProperties(name))
(parent, joint_properties, BodyNode::AspectProperties(name))
```

Now for the function arguments: The first specifies the parent BodyNode. In the
event that you want to create a root BodyNode, you can simply pass in a nullptr
as the parent. The second argument is a ``JointType::Properties`` struct, so we
pass in the ``properties`` object that we created earlier. The third argument is
pass in the ``joint_properties`` object that we created earlier. The third argument is
a ``BodyNode::Properties`` struct, but we're going to set the BodyNode properties
later, so we'll just toss the name in by wrapping it up in a
``BodyNode::AspectProperties`` object and leave the rest as default values.
Expand Down Expand Up @@ -230,8 +230,8 @@ Finally, we want to add this shape as a visualization **and** collision shape fo
the BodyNode:

```cpp
bn->addVisualizationShape(shape);
bn->addCollisionShape(shape);
auto shapeNode =
bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(shape);
```

We want to do this no matter which type was selected, so those two lines of code
Expand All @@ -257,7 +257,7 @@ bn->setInertia(inertia);
This is very easily done with the following function:

```cpp
bn->setRestitutionCoeff(default_restitution);
shapeNode->getDynamicsAspect()->setRestitutionCoeff(default_restitution);
```

### Lesson 1f: Set the damping coefficient
Expand Down Expand Up @@ -331,7 +331,6 @@ For the SOFT_BOX:
double width = default_shape_height, height = 2*default_shape_width;
Eigen::Vector3d dims(width, width, height);

Eigen::Vector3d dims(width, width, height);
double mass = 2*dims[0]*dims[1] + 2*dims[0]*dims[2] + 2*dims[1]*dims[2];
mass *= default_shape_density * default_skin_thickness;
soft_properties = SoftBodyNodeHelper::makeBoxProperties(
Expand Down Expand Up @@ -383,7 +382,7 @@ that we're creating a soft BodyNode. First, let's create a full
``SoftBodyNode::Properties``:

```cpp
SoftBodyNode::Properties body_properties(BodyNode::Properties(name),
SoftBodyNode::Properties body_properties(BodyNode::AspectProperties(name),
soft_properties);
```

Expand Down Expand Up @@ -424,9 +423,10 @@ will have exactly one visualization shape: the soft shape visualizer. We can
grab that shape and reduce the value of its alpha channel:

```
Eigen::Vector4d color = bn->getVisualizationShape(0)->getRGBA();
auto shape = bn->getShapeNodesWith<VisualAspect>()[0];
Eigen::Vector4d color = shape->getVisualAspect()->getRGBA();
color[3] = 0.4;
bn->getVisualizationShape(0)->setRGBA(color);
shape->getVisualAspect()->setRGBA(color);
```

### Lesson 2f: Give a hard bone to the SoftBodyNode
Expand All @@ -451,8 +451,7 @@ And then we can add that shape to the visualization and collision shapes of the
SoftBodyNode, just like normal:

```cpp
bn->addCollisionShape(box);
bn->addVisualizationShape(box);
bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(box);
```

And we'll want to make sure that we set the inertia of the underlying BodyNode,
Expand Down Expand Up @@ -487,8 +486,7 @@ double box_shape_height = default_shape_height;
std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(
box_shape_height*Eigen::Vector3d::Ones());

bn->addCollisionShape(box);
bn->addVisualizationShape(box);
bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(box);
```

To make the box protrude, we'll shift it away from the center of its parent:
Expand Down Expand Up @@ -560,7 +558,7 @@ We trust that the root Joint is a FreeJoint with 6 degrees of freedom because
of how we constructed all the objects that are going to be thrown at the wall:
They were all given a FreeJoint between the world and the root BodyNode.

### Lesson 3b: Add the object to the world
### Lesson 3b: Set the object's name

Every object in the world is required to have a non-empty unique name. Just like
Joint names in a Skeleton, if we pass a Skeleton into a world with a non-unique
Expand All @@ -571,54 +569,47 @@ ugly printout, we'll make sure the new object has a unique name ahead of time:
object->setName(object->getName()+std::to_string(mSkelCount++));
```

And now we can add it to the world without any complaints:
```cpp
mWorld->addSkeleton(object);
```
### Lesson 3c: Add the object to the world without collisions

### Lesson 3c: Compute collisions

Now that we've added the Skeleton to the world, we want to make sure that it
wasn't actually placed inside of something accidentally. If an object in a
Before we add the Skeleton to the world, we want to make sure that it
isn't actually placed inside of something accidentally. If an object in a
simulation starts off inside of another object, it can result in extremely
non-physical simulations, perhaps even breaking the simulation entirely.
We can access the world's collision detector directly to check make sure the
new object is collision-free:

```cpp
dart::collision::CollisionDetector* detector =
mWorld->getConstraintSolver()->getCollisionDetector();
detector->detectCollision(true, true);
auto collisionEngine
= mWorld->getConstraintSolver()->getCollisionDetector();
```

Now we shouldn't be surprised if the *other* objects are in collision with each
other, so we'll want to look through the list of collisions and check whether
any of them are the new Skeleton:
other, so we'll need to check whether our new object overlaps with any existing objects.
First, we use the collision engine to create a group which contains our object. Then,
we get a group containing the existing objects in the world and use it to check for
collisions.

```cpp
bool collision = false;
size_t collisionCount = detector->getNumContacts();
for(size_t i = 0; i < collisionCount; ++i)
{
const dart::collision::Contact& contact = detector->getContact(i);
if(contact.bodyNode1.lock()->getSkeleton() == object
|| contact.bodyNode2.lock()->getSkeleton() == object)
{
collision = true;
break;
}
}
auto newGroup = collisionEngine->createCollisionGroup(object.get());
auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup();

dart::collision::CollisionOption option;
dart::collision::CollisionResult result;
bool collision = collisionGroup->collide(newGroup.get(), option, &result);
```

If the new Skeleton *was* in collision with anything, we'll want to remove it
from the world and abandon our attempt to add it:
If the new skeleton doesn't overlap an existing object, we can add it to the
world without any complaints:

```cpp
if(collision)
if (!collision)
{
mWorld->addSkeleton(object);
}
else
{
mWorld->removeSkeleton(object);
std::cout << "The new object spawned in a collision. "
<< "It will not be added to the world." << std::endl;
<< "It will not be added to the world." << std::endl;
return false;
}
```
Expand Down Expand Up @@ -754,7 +745,7 @@ edge of a polygon like so:

```cpp
size_t numEdges = ring->getNumBodyNodes();
double angle = 2*M_PI/numEdges;
double angle = 2 * dart::math::constantsd::pi() / numEdges;
```

Now it's important to remember that the joints we have between the BodyNodes are
Expand Down
21 changes: 10 additions & 11 deletions docs/readthedocs/tutorials/multi-pendulum.md
Original file line number Diff line number Diff line change
Expand Up @@ -101,11 +101,10 @@ iterate through these two Shapes in each BodyNode, setting their colors to blue.
for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i)
{
BodyNode* bn = mPendulum->getBodyNode(i);
for(size_t j = 0; j < 2; ++j)
auto visualShapeNodes = bn->getShapeNodesWith<VisualAspect>();
for(std::size_t j = 0; j < 2; ++j)
{
const ShapePtr& shape = bn->getVisualizationShape(j);

shape->setColor(dart::Color::Blue());
visualShapeNodes[j]->getVisualAspect()->setColor(dart::Color::Blue());
}

// TODO: Remove any arrows
Expand All @@ -118,9 +117,9 @@ default, this arrow should not be attached, so in the outer for-loop, we should
check for arrows and remove them:

```cpp
if(bn->getNumVisualizationShapes() == 3)
if(visualShapeNodes.size() == 3)
{
bn->removeVisualizationShape(mArrow);
visualShapeNodes[2]->remove();
}
```

Expand Down Expand Up @@ -159,15 +158,15 @@ that corresponds to this Joint:

```cpp
BodyNode* bn = dof->getChildBodyNode();
const ShapePtr& shape = bn->getVisualizationShape(0);
auto shapeNodes = bn->getShapeNodesWith<VisualAspect>();
```

Because of the way the pendulum bodies were constructed, we trust that the
zeroth indexed visualization shape will be the shape that depicts the joint.
So now we will color it red:

```cpp
shape->setColor(dart::Color::Red());
shapeNodes[0]->getVisualAspect()->setColor(dart::Color::Red());
```

### Lesson 1c: Apply body forces based on user input
Expand Down Expand Up @@ -221,8 +220,8 @@ Now we'll want to visualize the force being applied to the body. First, we'll
grab the Shape for the body and color it red:

```cpp
const ShapePtr& shape = bn->getVisualizationShape(1);
shape->setColor(dart::Color::Red());
auto shapeNodes = bn->getShapeNodesWith<VisualAspect>();
shapeNodes[1]->getVisualAspect()->setColor(dart::Color::Red());
```

Last time we grabbed the 0-index visualization shape, because we trusted that
Expand All @@ -235,7 +234,7 @@ represent the applied force. The ``MyWindow`` class already provides the arrow
shape; we just need to add it:

```cpp
bn->addVisualizationShape(mArrow);
bn->createShapeNodeWith<VisualAspect>(mArrow);
```

# Lesson 2: Set spring and damping properties for joints
Expand Down