Skip to content

Commit

Permalink
Add joint force/torque getter (#1616)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 authored Nov 1, 2021
1 parent 0cc469b commit 4389b69
Show file tree
Hide file tree
Showing 6 changed files with 526 additions and 0 deletions.
4 changes: 4 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,10 @@

* Remove DART_BUILD_DARTPY option: [#1600](https://github.com/dartsim/dart/pull/1600)

* Dynamics

* Added joint force/torque getter: [#1616](https://github.com/dartsim/dart/pull/1616)

* Parsers

* Added default options to DartLoader for missing properties in URDF files: : [#1605](https://github.com/dartsim/dart/pull/1605)
Expand Down
46 changes: 46 additions & 0 deletions dart/dynamics/Joint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -387,6 +387,52 @@ const Eigen::Vector6d& Joint::getRelativePrimaryAcceleration() const
return mPrimaryAcceleration;
}

//==============================================================================
Eigen::Vector6d Joint::getWrenchToChildBodyNode(
const Frame* withRespectTo) const
{
const BodyNode* childBodyNode = getChildBodyNode();
if (!childBodyNode)
{
return Eigen::Vector6d::Zero();
}

const Eigen::Vector6d& F2 = childBodyNode->getBodyForce();
const BodyNode* parentBodyNode = getParentBodyNode();

if (withRespectTo == nullptr)
{
// (Default) Wrench applying to the child body node, where the reference
// frame is the joint frame
return math::dAdT(getTransformFromChildBodyNode(), -F2);
}
else if (withRespectTo == childBodyNode)
{
// Wrench applying to the child body node, where the reference frame is the
// child body frame
return -F2;
}
else if (withRespectTo == parentBodyNode)
{
// Wrench applying to the child body node, where the reference frame is the
// parent body frame
return math::dAdInvT(getRelativeTransform(), -F2);
}
else
{
// Wrench applying to the child body node, where the reference frame is an
// arbitrary frame
return math::dAdT(withRespectTo->getTransform(childBodyNode), -F2);
}
}

//==============================================================================
Eigen::Vector6d Joint::getWrenchToParentBodyNode(
const Frame* withRespectTo) const
{
return -getWrenchToChildBodyNode(withRespectTo);
}

//==============================================================================
void Joint::setPositionLimitEnforced(bool enforced)
{
Expand Down
25 changes: 25 additions & 0 deletions dart/dynamics/Joint.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include "dart/common/EmbeddedAspect.hpp"
#include "dart/common/Subject.hpp"
#include "dart/common/VersionCounter.hpp"
#include "dart/dynamics/Frame.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/detail/JointAspect.hpp"
#include "dart/math/MathTypes.hpp"
Expand Down Expand Up @@ -725,6 +726,30 @@ class Joint : public virtual common::Subject,
/// \sa BodyNode::updateArticulatedInertia(double).
// Eigen::VectorXd getDampingForces() const;

/// Returns wrench exerted to the child body node to satisfy the joint
/// constraint.
///
/// \param[in] withRespectTo: The reference frame where the wrench is
/// measured. The default (i.e., nullptr) is to get the wrench measured in the
/// joint frame.
/// \return Wrench where the first three elements represent torque and the
/// last three elements represent force. Zero vector if this joint has no
/// child body node defined.
Eigen::Vector6d getWrenchToChildBodyNode(
const Frame* withRespectTo = nullptr) const;

/// Returns wrench exerted to the parent body node to satisfy the joint
/// constraint.
///
/// \param[in] withRespectTo: The reference frame where the wrench is
/// measured. The default (i.e., nullptr) is to get the wrench measured in the
/// joint frame.
/// \return Wrench where the first three elements represent torque and the
/// last three elements represent force. Zero vector if this joint has no
/// child body node defined.
Eigen::Vector6d getWrenchToParentBodyNode(
const Frame* withRespectTo = nullptr) const;

//----------------------------------------------------------------------------
/// \{ \name Update Notifiers
//----------------------------------------------------------------------------
Expand Down
173 changes: 173 additions & 0 deletions data/sdf/test/force_torque_test.world
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,173 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<physics type="ode">
<gravity>0.000000 0.000000 -9.810000</gravity>
<ode>
<solver>
<type>quick</type>
<iters>1000</iters>
<precon_iters>0</precon_iters>
<sor>1.000000</sor>
</solver>
<constraints>
<cfm>0.000000</cfm>
<erp>0.200000</erp>
<contact_max_correcting_vel>100.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode>
<bullet>
<solver>
<type>sequential_impulse</type>
<iters>1000</iters>
<sor>1.000000</sor>
</solver>
<constraints>
<cfm>0.000000</cfm>
<erp>0.200000</erp>
<split_impulse>true</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<contact_surface_layer>0.00000</contact_surface_layer>
</constraints>
</bullet>
<real_time_update_rate>0.000000</real_time_update_rate>
<max_step_size>0.001</max_step_size>
</physics>
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>

<!-- to demonstrate force torque, we'll construct a model with
two bodies stacked vertically, with a x-revolute joint connecting
them. The joint has 90 degree limit. We'll test force
torque readings and characterize them. -->
<model name="model_1">
<link name="link_1">
<inertial>
<pose>0 0 0.5 0 0 0</pose>
<inertia>
<ixx>0.100000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.100000</iyy>
<iyz>0.000000</iyz>
<izz>0.100000</izz>
</inertia>
<mass>10.000000</mass>
</inertial>
<visual name="visual_sphere">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<sphere>
<radius>0.100000</radius>
</sphere>
</geometry>
</visual>
<collision name="collision_sphere">
<pose>0 0 0.5 0 0 0</pose>
<max_contacts>250</max_contacts>
<geometry>
<sphere>
<radius>0.100000</radius>
</sphere>
</geometry>
</collision>
</link>

<link name="link_2">
<pose>0 0 1.5 0 0 0</pose>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
<inertia>
<ixx>0.100000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.100000</iyy>
<iyz>0.000000</iyz>
<izz>0.100000</izz>
</inertia>
<mass>10.000000</mass>
</inertial>
<visual name="visual_box">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.2 0.4</size>
</box>
</geometry>
</visual>
<collision name="collision_box">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.2 0.4</size>
</box>
</geometry>
</collision>
</link>

<joint name="joint_01" type="revolute">
<parent>world</parent>
<child>link_1</child>
<!-- joint at origin of link_1 inertial frame -->
<!-- moement arm from link_1 inertial frame to joint_01 is 0m -->
<pose>0 0 0.5 0 0 0</pose>
<axis>
<limit>
<lower>-1.57079</lower>
<upper>1.57079</upper>
<effort>1000.000000</effort>
<velocity>1000.000000</velocity>
</limit>
<dynamics>
<damping>0.000000</damping>
<friction>0.000000</friction>
</dynamics>
<xyz>1.000000 0.000000 0.000000</xyz>
</axis>
<sensor name="force_torque" type="force_torque">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>30</update_rate>
</sensor>
<physics>
<provide_feedback>true</provide_feedback>
</physics>
</joint>

<joint name="joint_12" type="revolute">
<parent>link_1</parent>
<child>link_2</child>
<!-- joint_1 at origin of link_2 link frame -->
<!-- moement arm from link_2 inertial frame to joint_01 is 2m -->
<!-- moement arm from link_2 inertial frame to joint_12 is 0.5m -->
<pose>0 0 0 0 0 0</pose>
<axis>
<limit>
<lower>-0.000001</lower>
<upper>0.000001</upper>
<effort>1000.000000</effort>
<velocity>1000.000000</velocity>
</limit>
<dynamics>
<damping>0.000000</damping>
<friction>0.000000</friction>
</dynamics>
<xyz>0.000000 0.000000 1.000000</xyz>
</axis>
<sensor name="force_torque" type="force_torque">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>30</update_rate>
</sensor>
<physics>
<provide_feedback>true</provide_feedback>
</physics>
</joint>
</model>
</world>
</sdf>
3 changes: 3 additions & 0 deletions unittests/comprehensive/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,9 @@ if(TARGET dart-utils)
dart_add_test("comprehensive" test_Joints)
target_link_libraries(test_Joints dart-utils)

dart_add_test("comprehensive" test_JointForceTorque)
target_link_libraries(test_JointForceTorque dart-utils)

dart_add_test("comprehensive" test_Skeleton)
target_link_libraries(test_Skeleton dart-utils)

Expand Down
Loading

0 comments on commit 4389b69

Please sign in to comment.