revolute_joints.h

Go to the documentation of this file.

/**
 *  \file IMP/kinematics/revolute_joints.h
 *  \brief functionality for defining various revolute kinematic
 *         joints between rigid bodies as part of a kinematic tree,
 *         including RevoluteJoint, DihedralAngleRevoluteJoint, and
 *         BondAngleRevoluteJoint
 *  \authors Dina Schneidman, Barak Raveh
 *

 *  Copyright 2007-2017 IMP Inventors. All rights reserved.
 */

#ifndef IMPKINEMATICS_REVOLUTE_JOINTS_H
#define IMPKINEMATICS_REVOLUTE_JOINTS_H

#include "kinematics_config.h"
#include <IMP/kinematics/KinematicNode.h>
#include <IMP/kinematics/Joint.h>
#include <IMP/Object.h>
#include <IMP/exception.h>
#include <IMP/core/internal/dihedral_helpers.h>
#include <IMP/algebra/Vector3D.h>
#include <IMP/check_macros.h>

// TODO: for debug only = remove later
#define IMP_RAD_2_DEG(a) 180 * a / IMP::algebra::PI
#define IMP_DEG_2_RAD(a) a* IMP::algebra::PI / 180

IMPKINEMATICS_BEGIN_NAMESPACE

class KinematicForest;

// inline void nice_print_trans(const IMP::algebra::Transformation3D& T,
//                              std::string description)
// {
//   std::pair< IMP::algebra::Vector3D, double > aa;
//   aa = IMP::algebra::get_axis_and_angle( T.get_rotation() );
//   IMP_LOG( VERBOSE, description << "axis = " << aa.first
//            << "; angle = " << IMP_RAD_2_DEG(aa.second) << " deg"
//            << "; translation = " << T.get_translation()
//            << std::endl );
// }

/********************** RevoluteJoint ***************/

//! Abstract class for all revolute joints
class IMPKINEMATICSEXPORT RevoluteJoint : public Joint {
 public:
  //! Construct on the line connecting a and b, with an initial angle 'angle'
  /**
     @param parent,child kinematic nodes upstream and downstream (resp.) of this
                    joint
  **/
  RevoluteJoint(IMP::core::RigidBody parent, IMP::core::RigidBody child);

  // pure virtual dtr to declare as abstract class for SWIG
  virtual ~RevoluteJoint() = 0;

  /******* getter / setter methods *********/
 public:
  /**
     sets the angle of the revolute joint and marks the internal
     coordinates as changed in the kinematic forest object
   */
  void set_angle(double angle);

  /**
     gets the angle of the revolute joint. This method is kinematically
     safe (it triggers an update to internal coordinates if needed)
  */
  double get_angle() const;

 protected:
  //#ifndef SWIG
  // in global coordinates
  const IMP::algebra::Vector3D& get_rot_axis_origin() const {
    return rot_axis_origin_;
  }

  // in global coordinates
  const IMP::algebra::Vector3D& get_rot_axis_unit_vector() const {
    return rot_axis_unit_vector_;
  }
  //#endif

  /****************** general protected methods ***************/

 protected:
  /**
     Updates the reference frame of the child node by this joint
     angle, assuming the parent reference frame and the witnesses
     that affect update_axis_of_rotation_from_cartesian_witnesses()
     are all updated already
  */
  virtual void update_child_node_reference_frame() const;

  /**
      this protected method updates the rot_axis_unit_vector_
      and rot_axis_origin_ variables based on the Cartesian witnesses
      appropriate for a specific implementation of this abstract class,
      using parent coordinates, assuming all Cartesian witnesses are updated
  */
  virtual void update_axis_of_rotation_from_cartesian_witnesses() = 0;

  /**
      this protected method uses the Cartesian witnesses to compute
      the actual current angle of this joint (assuming external coordinates
      of required Cartesian witnesses are up to date).
      @note this method does not update the angle stored in this joint,
            which may be strictly different (if external or internal coords
            are outdated)
  */
  virtual double get_current_angle_from_cartesian_witnesses() const = 0;

  /**
     Update the joint internal parameters based on external reference frames
     of witnesses and rigid bodies, assuming external parameters are updated
   */
  virtual void update_joint_from_cartesian_witnesses() {
    update_axis_of_rotation_from_cartesian_witnesses();
    angle_ = get_current_angle_from_cartesian_witnesses();
    last_updated_angle_ = angle_;
    Joint::update_joint_from_cartesian_witnesses();
  }

  /**
     Returns the transformation matrix for rotating a vector in
     parent coordinates about the axis of the joint, in a way that
     would bring the Cartesian witnesses to the correct joint angle
     (as measured by get_angle_from_cartesian_witnesses() ).
   */
  IMP::algebra::Transformation3D
  get_rotation_about_joint_in_parent_coordinates() const {
    IMP_LOG(VERBOSE, "get_rotation " << IMP_RAD_2_DEG(angle_)
                                     << ", last_updated_angle = "
                                     << IMP_RAD_2_DEG(last_updated_angle_)
                                     << std::endl);
    // rotate by the difference from last_updated_angle_
    IMP::algebra::Rotation3D R =
        IMP::algebra::get_rotation_about_normalized_axis(
            rot_axis_unit_vector_, angle_ - last_updated_angle_);
    IMP::algebra::Transformation3D R_origin =
        IMP::algebra::get_rotation_about_point(rot_axis_origin_, R);

    // debug prints
    // nice_print_trans(R_origin, "R_origin: ");

    return R_origin;
  }

 protected:
  // the angle in Radians about the joint axis ("unit vector")
  double angle_;

  // the angle used when the child reference frame was last up-to-date
  // mutable cause logically does not change the state of this object
  // (or is that so?)
  mutable double last_updated_angle_;

  // the unit vector around which the joint revolves in parent coords
  IMP::algebra::Vector3D rot_axis_unit_vector_;

  // the joint origin of rotation in parent coords
  IMP::algebra::Vector3D rot_axis_origin_;
};

/********************** DihedralAngleRevoluteJoint ***************/
//     @TODO handle derivatives

//! Joint that is parameterized as a dihedral angle between two planes
class IMPKINEMATICSEXPORT DihedralAngleRevoluteJoint : public RevoluteJoint {
 public:
  /**
     constructs a dihedral angle that revolves around the axis b-c,
     using a,b,c,d as witnesses for the dihedral angle

     @param parent,child kinematic nodes upstream and downstream (resp.) of
                    this joint
     @param a,b,c,d 'witnesses' whose coordinates define the dihedral
                    angle between the planes containing a-b-c and
                    b-c-d)

     @note It is assumed that neither a, b and c are downstream of child,
           and also that d is not upstream of it
     */
  DihedralAngleRevoluteJoint(IMP::core::RigidBody parent,
                             IMP::core::RigidBody child, IMP::core::XYZ a,
                             IMP::core::XYZ b, IMP::core::XYZ c,
                             IMP::core::XYZ d);

  IMP::core::XYZ get_a() const { return a_; }
  IMP::core::XYZ get_b() const { return b_; }
  IMP::core::XYZ get_c() const { return c_; }
  IMP::core::XYZ get_d() const { return d_; }

 protected:
  /**
      updates the rot_axis_unit_vector_ and rot_axis_origin_ variables
      in parent coordinates based on the witnesses b_ and c_,
      using b_-c_ as the axis of rotation
      @note it is assumed b_ and c_ have update Cartesian coordinates
  */
  virtual void update_axis_of_rotation_from_cartesian_witnesses() {
    using namespace IMP::algebra;
    IMP_USAGE_CHECK(
        get_distance(b_.get_coordinates(), c_.get_coordinates()) > 1e-12,
        "witnesses b and c must be non identical beyond numerical error");
    ReferenceFrame3D rf_parent = get_parent_node().get_reference_frame();
    // nice_print_trans(rf_parent.get_transformation_to(), "Parent trans: ");
    rot_axis_origin_ = rf_parent.get_local_coordinates(b_.get_coordinates());
    IMP_LOG(VERBOSE, "global b_ " << b_.get_coordinates()
                                  << " and local parent b_ " << rot_axis_origin_
                                  << std::endl);
    Vector3D v = rf_parent.get_local_coordinates(c_.get_coordinates()) -
                 rf_parent.get_local_coordinates(b_.get_coordinates());
    rot_axis_unit_vector_ = v.get_unit_vector();
    IMP_LOG(VERBOSE, "local axis of rot unnorm "
                         << v << " global axis "
                         << c_.get_coordinates() - b_.get_coordinates()
                         << std::endl);
  };

  /**
      this protected method uses the Cartesian witnesses to compute
      the actual current dihedral angle of this joint (assuming
      external coordinates of required Cartesian witnesses are up to
      date)
  */
  virtual double get_current_angle_from_cartesian_witnesses() const;

 private:
  IMP::core::XYZ a_;
  IMP::core::XYZ b_;
  IMP::core::XYZ c_;
  IMP::core::XYZ d_;
};

/********************** BondAngleRevoluteJoint ***************/
// TODO: do we want to handle derivatives?

//! Joint that is parameterized as a bond angle between three particles
class IMPKINEMATICSEXPORT BondAngleRevoluteJoint : public RevoluteJoint {
 public:
  /**
     constructs a joint that controls the angle a-b-c. The joint
     revolves around the axis that passes through b, normal to the
     plane containing a, b and c. a,b and c are the witnesses for the
     bond angle.

     @param parent,child kinematic nodes upstream and downstream (resp.) of
                    this joint
     @param a,b,c 'witnesses' whose coordinates define the joint angle a-b-c

     @note It is assumed that a and b are upstream of or inside
           this joint's child rigid body, and that c is downstream of
           it or inside it.
  */
  BondAngleRevoluteJoint(IMP::core::RigidBody parent,
                         IMP::core::RigidBody child, IMP::core::XYZ a,
                         IMP::core::XYZ b, IMP::core::XYZ c);

 protected:
  /**
     this protected method uses the Cartesian witnesses to compute the
     actual current bond angle of this joint (assuming external
     coordinates of required Cartesian witnesses are up to date)
  */
  virtual double get_current_angle_from_cartesian_witnesses() const;

  /**
     updates the rot_axis_unit_vector_ and rot_axis_origin_ variables
     using b_ as origin of rotation and a vector perpendicular to the
     plane containing a_,b_,c_ as axis of rotation, in global
     coordinates
  */
  virtual void update_axis_of_rotation_from_cartesian_witnesses() {
    using namespace IMP::algebra;

    IMP_USAGE_CHECK(
        get_distance(b_.get_coordinates(), c_.get_coordinates()) > 1e-12,
        "witnesses b and c must be non identical beyond numerical error");
    IMP_USAGE_CHECK(
        get_distance(b_.get_coordinates(), a_.get_coordinates()) > 1e-12,
        "witnesses b and a must be non identical beyond numerical error");
    ReferenceFrame3D rf_parent = get_parent_node().get_reference_frame();
    Vector3D v =
        rf_parent.get_local_coordinates(get_perpendicular_vector(a_, b_, c_));
    rot_axis_unit_vector_ = v.get_unit_vector();
    rot_axis_origin_ = rf_parent.get_local_coordinates(b_.get_coordinates());
  };

 private:
  static IMP::algebra::Vector3D get_perpendicular_vector(core::XYZ a,
                                                         core::XYZ b,
                                                         core::XYZ c) {
    IMP::algebra::Vector3D v1 = a.get_coordinates() - b.get_coordinates();
    IMP::algebra::Vector3D v2 = c.get_coordinates() - b.get_coordinates();
    return IMP::algebra::get_vector_product(v1, v2);
  }
  IMP::core::XYZ a_;
  IMP::core::XYZ b_;
  IMP::core::XYZ c_;
};

IMP_OBJECTS(RevoluteJoint, RevoluteJoints);
IMP_OBJECTS(DihedralAngleRevoluteJoint, DihedralAngleRevoluteJoints);
IMP_OBJECTS(BondAngleRevoluteJoint, BondAngleRevolteJoints);

IMPKINEMATICS_END_NAMESPACE

#endif /* IMPKINEMATICS_REVOLUTE_JOINTS_H */