1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <sstream>
34
35 #include "Helpers.hpp"
36
37 #include <dart/utils/urdf/urdf.hpp>
38
39 //==============================================================================
createGround()40 dart::dynamics::SkeletonPtr createGround()
41 {
42 // Create a Skeleton to represent the ground
43 dart::dynamics::SkeletonPtr ground
44 = dart::dynamics::Skeleton::create("ground");
45 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
46 double thickness = 0.01;
47 tf.translation() = Eigen::Vector3d(0, 0, -thickness / 2.0);
48 dart::dynamics::WeldJoint::Properties joint;
49 joint.mT_ParentBodyToJoint = tf;
50 ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>(nullptr, joint);
51 dart::dynamics::ShapePtr groundShape
52 = std::make_shared<dart::dynamics::BoxShape>(
53 Eigen::Vector3d(10, 10, thickness));
54
55 auto shapeNode = ground->getBodyNode(0)
56 ->createShapeNodeWith<
57 dart::dynamics::VisualAspect,
58 dart::dynamics::CollisionAspect,
59 dart::dynamics::DynamicsAspect>(groundShape);
60 shapeNode->getVisualAspect()->setColor(dart::Color::Blue(0.2));
61
62 return ground;
63 }
64
65 //==============================================================================
createWam()66 dart::dynamics::SkeletonPtr createWam()
67 {
68 dart::utils::DartLoader urdfParser;
69 urdfParser.addPackageDirectory(
70 "herb_description", DART_DATA_PATH "/urdf/wam");
71 dart::dynamics::SkeletonPtr wam
72 = urdfParser.parseSkeleton(DART_DATA_PATH "/urdf/wam/wam.urdf");
73
74 return wam;
75 }
76
77 //==============================================================================
setStartupConfiguration(const dart::dynamics::SkeletonPtr & wam)78 void setStartupConfiguration(const dart::dynamics::SkeletonPtr& wam)
79 {
80 wam->getDof("/j1")->setPosition(0.0);
81 wam->getDof("/j2")->setPosition(0.0);
82 wam->getDof("/j3")->setPosition(0.0);
83 wam->getDof("/j4")->setPosition(0.0);
84 wam->getDof("/j5")->setPosition(0.0);
85 wam->getDof("/j6")->setPosition(0.0);
86 wam->getDof("/j7")->setPosition(0.0);
87 }
88
89 //==============================================================================
setupEndEffectors(const dart::dynamics::SkeletonPtr & wam)90 void setupEndEffectors(const dart::dynamics::SkeletonPtr& wam)
91 {
92 Eigen::Vector3d linearBounds
93 = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
94
95 Eigen::Vector3d angularBounds
96 = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
97
98 Eigen::Isometry3d tf_hand(Eigen::Isometry3d::Identity());
99 tf_hand.translate(Eigen::Vector3d(0.0, 0.0, -0.09));
100
101 dart::dynamics::EndEffector* ee
102 = wam->getBodyNode("/wam7")->createEndEffector("ee");
103 ee->setDefaultRelativeTransform(tf_hand, true);
104
105 auto wam7_target = std::make_shared<dart::gui::osg::InteractiveFrame>(
106 dart::dynamics::Frame::World(), "lh_target");
107
108 ee->getIK(true)->setTarget(wam7_target);
109
110 std::stringstream ss;
111 ss << DART_SHARED_LIB_PREFIX << "wamIk";
112 #if (DART_OS_LINUX || DART_OS_FREEBSD || DART_OS_MACOS) && !NDEBUG
113 ss << "d";
114 #endif
115 ss << "." << DART_SHARED_LIB_EXTENSION;
116 std::string libName = ss.str();
117
118 std::vector<std::size_t> ikFastDofs{0, 1, 3, 4, 5, 6};
119 std::vector<std::size_t> ikFastFreeDofs{2};
120 ee->getIK()->setGradientMethod<dart::dynamics::SharedLibraryIkFast>(
121 libName, ikFastDofs, ikFastFreeDofs);
122
123 ee->getIK()->getErrorMethod().setLinearBounds(-linearBounds, linearBounds);
124 ee->getIK()->getErrorMethod().setAngularBounds(-angularBounds, angularBounds);
125 }
126
127 //==============================================================================
enableDragAndDrops(dart::gui::osg::Viewer & viewer,const dart::dynamics::SkeletonPtr & wam)128 void enableDragAndDrops(
129 dart::gui::osg::Viewer& viewer, const dart::dynamics::SkeletonPtr& wam)
130 {
131 // Turn on drag-and-drop for the whole Skeleton
132 for (std::size_t i = 0; i < wam->getNumBodyNodes(); ++i)
133 viewer.enableDragAndDrop(wam->getBodyNode(i), false, false);
134
135 for (std::size_t i = 0; i < wam->getNumEndEffectors(); ++i)
136 {
137 dart::dynamics::EndEffector* ee = wam->getEndEffector(i);
138 if (!ee->getIK())
139 continue;
140
141 // Check whether the target is an interactive frame, and add it if it is
142 const auto& frame
143 = std::dynamic_pointer_cast<dart::gui::osg::InteractiveFrame>(
144 ee->getIK()->getTarget());
145
146 if (frame)
147 viewer.enableDragAndDrop(frame.get());
148 }
149 }
150