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.

Sign up for GitHub

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

Jump to bottom

Add joint force/torque getter #1616

Merged
merged 2 commits into from
Nov 1, 2021
Merged

Add joint force/torque getter #1616

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6e5488f
Add joint force/torque getter
jslee02 Oct 30, 2021
70cd90a
Update changelog
jslee02 Nov 1, 2021
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
4 changes: 4 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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