SForce.hpp

/* This file is part of scl, a control and simulation library
for robots and biomechanical models.

scl is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

Alternatively, you can redistribute it and/or
modify it under the terms of the GNU General Public License as
published by the Free Software Foundation; either version 2 of
the License, or (at your option) any later version.

scl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License and a copy of the GNU General Public License along with
scl. If not, see <http://www.gnu.org/licenses/>.
*/
/* \file SForce.hpp
 *
 *  Created on: Mar 23, 2011
 *
 *  Copyright (C) 2011
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef SFORCE_HPP_
#define SFORCE_HPP_

#include <scl/DataTypes.hpp>
#include <scl/data_structs/SObject.hpp>
#include <scl/data_structs/SRobotParsed.hpp>
#include <scl/data_structs/SRigidBodyDyn.hpp>

#include <Eigen/Dense>
#include <string>

namespace scl
{
  struct SForce : public SObject
  {
    // Eigen requires redefining the new operator for classes that contain fixed size Eigen member-data.
    // See http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    const SRobotParsed *robot_;

    std::string link_name_;

    const SRigidBodyDyn *rbd_;

    Eigen::Vector6d force_;

    Eigen::Vector3d pos_;

    Eigen::Vector3d direction_;

    Eigen::MatrixXd J_;

    SForce(): SObject("SForce"), robot_(NULL),link_name_(""),
        rbd_(NULL){}

    virtual ~SForce(){}

  protected:
    SForce(std::string arg_subclass_type): SObject(arg_subclass_type),
        robot_(NULL),link_name_(""), rbd_(NULL){}
  };

  struct SForceContact : public SForce
  {
    // Eigen requires redefining the new operator for classes that contain fixed size Eigen member-data.
    // See http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    const SRobotParsed *other_robot_;

    std::string other_link_name_;

    const SRigidBodyDyn *other_rbd_;

    Eigen::MatrixXd other_J_;

    Eigen::Vector6d p_pre_, other_p_pre_;

    Eigen::Vector6d p_curr_, other_p_curr_;

    sFloat coeff_restitution_;

    sFloat time_impulse_;

    sFloat energy_collision_;

    SForceContact(): SForce("SForceContact"), other_robot_(NULL), other_link_name_(""),
        other_rbd_(NULL), coeff_restitution_(-1), time_impulse_(-1),
        energy_collision_(std::numeric_limits<sFloat>::quiet_NaN()){}

    virtual ~SForceContact(){}

  protected:
    SForceContact(std::string arg_subclass_type): SForce(arg_subclass_type),
    other_robot_(NULL), other_link_name_(""),
    other_rbd_(NULL), coeff_restitution_(-1), time_impulse_(-1),
    energy_collision_(std::numeric_limits<sFloat>::quiet_NaN()) {}
  };
}

#endif /* SFORCE_HPP_ */