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 "InputHandler.hpp"
34
35 #include "WamWorld.hpp"
36
37 //==============================================================================
InputHandler(dart::gui::osg::Viewer * viewer,WamWorld * teleop,const SkeletonPtr & wam,const WorldPtr & world)38 InputHandler::InputHandler(
39 dart::gui::osg::Viewer* viewer,
40 WamWorld* teleop,
41 const SkeletonPtr& wam,
42 const WorldPtr& world)
43 : mViewer(viewer), mWamWorld(teleop), mWam(wam), mWorld(world)
44 {
45 initialize();
46 }
47
48 //==============================================================================
initialize()49 void InputHandler::initialize()
50 {
51 mRestConfig = mWam->getPositions();
52
53 for (std::size_t i = 0; i < mWam->getNumEndEffectors(); ++i)
54 {
55 const InverseKinematicsPtr ik = mWam->getEndEffector(i)->getIK();
56 if (ik)
57 {
58 mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
59 mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
60 mConstraintActive.push_back(false);
61 mEndEffectorIndex.push_back(i);
62 }
63 }
64 }
65
66 //==============================================================================
handle(const osgGA::GUIEventAdapter & ea,osgGA::GUIActionAdapter &)67 bool InputHandler::handle(
68 const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&)
69 {
70 if (!mWam)
71 return false;
72
73 if (::osgGA::GUIEventAdapter::KEYDOWN == ea.getEventType())
74 {
75 if (ea.getKey() == 'p' || ea.getKey() == 'P')
76 {
77 for (std::size_t i = 0; i < mWam->getNumDofs(); ++i)
78 std::cout << mWam->getDof(i)->getName() << ": "
79 << mWam->getDof(i)->getPosition() << std::endl;
80 return true;
81 }
82
83 if (ea.getKey() == 't' || ea.getKey() == 'T')
84 {
85 mWam->setPositions(mRestConfig);
86 return true;
87 }
88
89 if ('1' <= ea.getKey() && ea.getKey() <= '9')
90 {
91 std::size_t index = ea.getKey() - '1';
92 if (index < mConstraintActive.size())
93 {
94 EndEffector* ee = mWam->getEndEffector(mEndEffectorIndex[index]);
95 const InverseKinematicsPtr& ik = ee->getIK();
96 if (ik && mConstraintActive[index])
97 {
98 mConstraintActive[index] = false;
99
100 ik->getErrorMethod().setBounds(mDefaultBounds[index]);
101 ik->getTarget()->setRelativeTransform(mDefaultTargetTf[index]);
102 mWorld->removeSimpleFrame(ik->getTarget());
103 }
104 else if (ik)
105 {
106 mConstraintActive[index] = true;
107
108 // Use the standard default bounds instead of our custom default
109 // bounds
110 ik->getErrorMethod().setBounds();
111 ik->getTarget()->setTransform(ee->getTransform());
112 mWorld->addSimpleFrame(ik->getTarget());
113 }
114 }
115 return true;
116 }
117 }
118
119 return false;
120 }
121