Operational space control example

apps/CMakeLists.txt

@@ -6,12 +6,13 @@ foreach(APPDIR
     addDeleteSkels
     atlasSimbicon
     bipedStand
-    rigidLoop
-    rigidCubes
-    rigidChain
-    hardcodedDesign
+    hardcodedDesign    
     jointConstraints
     mixedChain
+    operationalSpaceControl
+    rigidChain
+    rigidCubes
+    rigidLoop    
     softBodies
     vehicle
     )

apps/operationalSpaceControl/CMakeLists.txt

@@ -0,0 +1,7 @@
+###############################################
+# apps/operationalSpaceControl
+file(GLOB operationalSpaceControl_srcs "*.cpp")
+file(GLOB operationalSpaceControl_hdrs "*.h")
+add_executable(operationalSpaceControl ${operationalSpaceControl_srcs} ${operationalSpaceControl_hdrs})
+target_link_libraries(operationalSpaceControl dart)
+set_target_properties(operationalSpaceControl PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin")

apps/operationalSpaceControl/Controller.cpp

@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2014, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "apps/operationalSpaceControl/Controller.h"
+
+#include "dart/dynamics/Skeleton.h"
+#include "dart/dynamics/BodyNode.h"
+#include "dart/dynamics/Joint.h"
+#include "dart/simulation/World.h"
+
+//==============================================================================
+Controller::Controller(dart::dynamics::Skeleton* _robot,
+                       dart::dynamics::BodyNode* _endEffector)
+  : mRobot(_robot),
+    mEndEffector(_endEffector)
+{
+  assert(_robot != NULL);
+  assert(_endEffector != NULL);
+
+  int dof = mRobot->getNumDofs();
+
+  mForces.setZero(dof);
+
+  mKp.setZero(dof, dof);
+  mKv.setZero(dof, dof);
+
+  for (int i = 0; i < dof; ++i)
+  {
+    mKp(i, i) = 750.0;
+    mKv(i, i) = 250.0;
+  }
+
+  // Remove position limits
+  for (int i = 0; i < dof; ++i)
+    _robot->getJoint(i)->setPositionLimited(false);
+
+  // Set joint damping
+  for (int i = 0; i < dof; ++i)
+    _robot->getJoint(i)->setDampingCoefficient(0, 0.5);
+}
+
+//==============================================================================
+Controller::~Controller()
+{
+}
+
+//==============================================================================
+void Controller::update(const Eigen::Vector3d& _targetPosition)
+{
+  // Get equation of motions
+  Eigen::Vector3d x    = mEndEffector->getTransform().translation();
+  Eigen::Vector3d dx   = mEndEffector->getWorldLinearVelocity();
+  Eigen::MatrixXd invM = mRobot->getInvMassMatrix();                   // n x n
+  Eigen::VectorXd Cg   = mRobot->getCoriolisAndGravityForces();        // n x 1
+  Eigen::MatrixXd Jv   = mEndEffector->getWorldLinearJacobian();       // 3 x n
+  Eigen::MatrixXd dJv  = mEndEffector->getWorldLinearJacobianDeriv();  // 3 x n
+  Eigen::VectorXd dq   = mRobot->getVelocities();                      // n x 1
+
+  // Compute operational space values
+  Eigen::MatrixXd A = Jv*invM;                 // 3 x n
+  Eigen::Vector3d b = /*-(A*Cg) + */dJv*dq;    // 3 x 1
+  Eigen::MatrixXd M2 = Jv*invM*Jv.transpose(); // 3 x 3
+
+  // Compute virtual operational space spring force at the end effector
+  Eigen::Vector3d f = -mKp*(x - _targetPosition) - mKv*dx;
+
+  // Compute desired operational space acceleration given f
+  Eigen::Vector3d desired_ddx = b + M2*f;
+
+  // Gravity compensation
+  mForces = Cg;
+
+  // Compute joint space forces to acheive the desired acceleration by solving:
+  // A tau + b = desired_ddx
+  mForces += A.colPivHouseholderQr().solve(desired_ddx - b);
+
+  // Apply the joint space forces to the robot
+  mRobot->setForces(mForces);
+}
+
+//==============================================================================
+dart::dynamics::Skeleton* Controller::getRobot() const
+{
+  return mRobot;
+}
+
+//==============================================================================
+dart::dynamics::BodyNode* Controller::getEndEffector() const
+{
+  return mEndEffector;
+}
+
+//==============================================================================
+void Controller::keyboard(unsigned char _key, int _x, int _y)
+{
+}
+

apps/operationalSpaceControl/Controller.h

@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2014, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef APPS_OPERATIONALSPACECONTROL_CONTROLLER_H_
+#define APPS_OPERATIONALSPACECONTROL_CONTROLLER_H_
+
+#include <Eigen/Eigen>
+
+#include "dart/gui/SimWindow.h"
+#include "dart/dynamics/Skeleton.h"
+
+/// \brief Operational space controller for 6-dof manipulator
+class Controller
+{
+public:
+  /// \brief Constructor
+  Controller(dart::dynamics::Skeleton* _robot,
+             dart::dynamics::BodyNode* _endEffector);
+
+  /// \brief Destructor
+  virtual ~Controller();
+
+  /// \brief
+  void update(const Eigen::Vector3d& _targetPosition);
+
+  /// \brief Get robot
+  dart::dynamics::Skeleton* getRobot() const;
+
+  /// \brief Get end effector of the robot
+  dart::dynamics::BodyNode* getEndEffector() const;
+
+  /// \brief Keyboard control
+  virtual void keyboard(unsigned char _key, int _x, int _y);
+
+private:
+  /// \brief Robot
+  dart::dynamics::Skeleton* mRobot;
+
+  /// \brief End-effector of the robot
+  dart::dynamics::BodyNode* mEndEffector;
+
+  /// \brief Control forces
+  Eigen::VectorXd mForces;
+
+  /// \brief Proportional gain for the virtual spring forces at the end effector
+  Eigen::MatrixXd mKp;
+
+  /// \brief Derivative gain for the virtual spring forces at the end effector
+  Eigen::MatrixXd mKv;
+};
+
+#endif  // APPS_OPERATIONALSPACECONTROL_CONTROLLER_H_

apps/operationalSpaceControl/Main.cpp

@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2014, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Jeongseok Lee <jslee02@gmail.com>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+ * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "dart/utils/Paths.h"
+#include "dart/math/Helpers.h"
+#include "dart/dynamics/Skeleton.h"
+#include "dart/simulation/World.h"
+#include "dart/utils/SkelParser.h"
+#include "dart/utils/urdf/DartLoader.h"
+
+#include "apps/operationalSpaceControl/MyWindow.h"
+
+int main(int argc, char* argv[])
+{
+  // create and initialize the world
+  dart::simulation::World* world = new dart::simulation::World;
+  assert(world != NULL);
+
+  // load skeletons
+  dart::utils::DartLoader dl;
+  dart::dynamics::Skeleton* ground
+          = dl.parseSkeleton(DART_DATA_PATH"urdf/KR5/ground.urdf");
+  dart::dynamics::Skeleton* robot
+          = dl.parseSkeleton(DART_DATA_PATH"urdf/KR5/KR5 sixx R650.urdf");
+  world->addSkeleton(ground);
+  world->addSkeleton(robot);
+
+  // create and initialize the world
+  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
+  world->setGravity(gravity);
+  world->setTimeStep(1.0/1000);
+
+  // create a window and link it to the world
+  MyWindow window(new Controller(robot, robot->getBodyNode("palm")));
+  window.setWorld(world);
+
+  glutInit(&argc, argv);
+  window.initWindow(640, 480, "Forward Simulation");
+  glutMainLoop();
+
+  delete world;
+
+  return 0;
+}