Question regarding BallJoint behaviour

Hello all, I’m currently having an issue with the spherical joint. I’d like to place a capsule of height 1.0 and radius 0.1 at position (1.0, 0.0, 1.0) with a ball-joint at the top corner of the capsule, so it’s like a pole that can swing around the pivot point (1.0, 0.0, 1.5). Unfortunately, the position at which the capsule is placed is not the one I was expecting, as it’s placed at (1.0, 0.0, 0.5), and the pivot ends up at world-position (1.0, 0.0, 1.0). It seems that the balljoint is forcing the capsule to move to that position.

The code I’m currently using to create the joint-bodynode pair for the balljoint is the following:

        const std::string body_name = m_ConstraintRef->parent()->name();
        const std::string joint_name = m_ConstraintRef->name();

        // THIS BELOW RETURNS jnt_pivot = { 0.0, 0.0, 0.5 }
        const Eigen::Vector3d jnt_pivot = vec3_to_eigen( TVec3( m_ConstraintRef->local_tf().col( 3 ) ) );

        dart::dynamics::BallJoint::Properties jnt_properties;
        jnt_properties.mName = joint_name;
        jnt_properties.mT_ChildBodyToJoint.translation() = jnt_pivot;

        auto joint_bodynode_pair = m_DartSkeletonRef->createJointAndBodyNodePair<dart::dynamics::BallJoint>(
                                                        nullptr, jnt_properties, dart::dynamics::BodyNode::AspectProperties( body_name ) );

The placing of the capsule is set by the setTransformFromParentBodyNode method, as shown below (where dart-tf is the transform por a translation to (1.0, 0.0, 1.0)):

    m_DartJointRef->setTransformFromParentBodyNode( dart_tf );

Perhaps I’m making a silly mistake. but so far I’ve tried various changes to no avail. Is the ChildBodyToJont transform the transform of the child-body’s frame (this-body) w.r.t. the joint’s frame?. I’ve tried the opposite and it when even under the plane (I though that it should be the opposite way).

Below there’s an image of the result in simulation using DART (BallJoint):

And below there’s the expected results using BULLET (btPoint2PointConstraint):

Update: Now that I notice, even the revolute constraints are placed in a different position. In both Mujoco and Bullet, the position of the rectangle-pole in the plane rotates around the pivot point (0.0, 1.0, 0.0), but in DART it rotates around the pivot point (-0.5, 1.0, 0.0) :worried:. It’s as if they were pushed by 0.5 in their local z-axis (for both revolute and ball joints)

It looks like you’re not setting the parent-to-joint transform, which is going to be important in this use case.

I’m not sure how exactly the code you’re showing is getting you the results that you’re showing (if jnt_pivot is really [0.0, 0.0, 0.5] then I don’t know how how your capsule is offset along the x-axis). But I believe the following code snippet should get you what you want:

dart::dynamics::BallJoint::Properties jnt_properties;
jnt_properties.mName = joint_name;
jnt_properties.mT_ChildBodyToJoint.translation()[2] = 0.5;
jnt_properties.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(1.0, 0.0, 1.0);

/* ... rest of your code... */

When there is no parent body specified, the world transform is used as the parent transformation. The way your code is set up now, I believe the joint will always be pivoting around the world origin.

Thanks @grey for the response. I’ll give the snippet a try. The setup is meant for the capsule to have no parent, and make it pivot around its top corner with a ball joint. I did set the parent-to-joint transform, in other part of the codebase using setTransformFromParentBodyNode, which seems to be the same as using properties.mT_ParentBodyToJoint. However, the order might be relevant, as I’m creating the constraint first, and then setting the parent transform.

If I wasn’t setting the parent-to-joint transform, the capsule would indeed be at the origin, but the balljoint should be at the top above the origin, right?, at the position given by child-to-joint-transform?

Thanks again for the reply, I’ll give the snippet a try and see if that makes the trick.

Because of how the forward kinematics are calculated, the location of the joint depends 100% on

  1. The transform of the parent body
  2. The value of mT_ParentBodyToJoint
  3. The BallJoint position values

The mT_ChildBodyToJoint value simply determines where the child body is relative to the joint. Changing that value will not change where the joint is. The name of that variable is a bit counter-intuitive, because it sort of implies that it’s a dependency of the joint location instead of a dependency of the child body location, but I think the naming of that variable is a legacy from a time when it made more sense for the API.

I’m not sure what m_ConstraintRef is referring to or how it’s being used, so I don’t know what kind of effect it could be having on the simulation results.

1 Like

Oh sorry, I just realized that I misinterpreted your diagram. You should actually do this:

jnt_properties.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(1.0, 0.0, 1.5);

Note the 1.5 instead of 1.0.

1 Like

Thanks @grey :smiley:, I’ve made a quick fix using the following snippet:

const Eigen::Vector3d jnt_pivot_to_this_body = ... // ( 0.0, 0.0, 0.5 )
const Eigen::Vector3d jnt_pivot_to_world = ... // ( 1.0, 0.0, 0.5 )

dart::dynamics::BallJoint::Properties jnt_properties;
jnt_properties.mName = joint_name;
jnt_properties.mT_ChildBodyToJoint.translation() = jnt_pivot_to_this_body;
jnt_properties.mT_ParentBodyToJoint.translation() = jnt_pivot_to_world;

I removed the call to setTransformFromParentBodyNode that I was using in other part of the code that was supposed to “set the world-transform of the body” (grabbed from a snippet that had a free-joint centered at COM), but I think that actually uses mT_ParentBodyToJoint as well :face_with_hand_over_mouth: . I thought the setup was for the COM of the body, but it seems it’s actually a setup for the joint transform, so I had to compensate for that xD.

Is there a method that can set the position of a body as if it were a maximal coordinates API?. I think Bullet bodies use maximal for primitives and minimal for articulated bodies, but in DART all is worked in minimal using Roy Featherstone’s representation, right?. I’ve made a quick dirty hack and the joint is working :smiley:, but it’d be better if I could replace the call to joint->setTransformFromParentBodyNode with something like body->setWorldTransform.

Thanks a lot for the help :+1:

Is there a method that can set the position of a body as if it were a maximal coordinates API?

There isn’t an explicit API for this. One approach you could consider doing is creating all bodies initially with FreeJoints, then have a function that uses BodyNode::moveTo<JointType>(JointType::Properties) to change the joint type. You’d want to set JointType::Properties::mT_ParentBodyToJoint and JointType::Properties::mT_ChildBodyToJoint based on the current transforms of the bodies (using BodyNode::getWorldTransform()). It would take a little bit of kinematics, but Eigen::Isometry3d::inverse() is very useful for that.

1 Like

Thanks @grey, I’ll use your recommended apporoach :+1: