Update constraint kinematics for rigid body tree model

[MatthewChignoli]

Our quick fix for dealing with constraint jacobians that depend on the robot's state (e.g., the Tello Joint) is to call extractLoopClosureFunctionsFromClusterModel() before running the forward dynamics. This works well but (i) it is inefficient and (ii) it makes extractLoopClosureFunctionsFromClusterModel() a public function that we probably want to be private, and (iii) it is prone to bugs because it requires remembering to call extractLoopClosureFunctionsFromClusterModel() rather than having it be done automatically.

Here are some options I can see for cleaning this up:

1. Use lambda functions. So similar to how we use the lambda function for gamma_, we will do for G, g, K, k, phi, etc
   

   generalized_rbda/Dynamics/RigidBodyTreeModel.cpp

   Lines 41 to 52 in 53d02b7

   	gamma_ = [cluster_tree_model](DVec<double> q)
   	{
   	DVec<double> q_full = DVec<double>::Zero(0);
   	for (const auto &cluster : cluster_tree_model.clusters())
   	{
   	const int pos_idx = cluster->position_index_;
   	const int num_pos = cluster->num_positions_;
   	const auto joint = cluster->joint_;
   	q_full = appendEigenVector(q_full, joint->gamma(q.segment(pos_idx, num_pos)));
   	}
   	return q_full;
   	};

2. Have RigidBodyTreeModel override the forwardKinematics function so that it looks like this

void RigidBodyTreeModel::forwadKinematics() override
{
extractLoopClosureFunctionsFromClusterModel();
TreeModel::forwardKinematics();
}

Option 1 is probably better, but more difficult to implement. Option 2 is a little cleaner because it eliminates the problem of having to remember to call extractLoopClosureFunctionsFromClusterModel(), but will probably require that RigidBodyTreeModel keep a cluster tree model as a member variable, which seems uneccesary.

[MatthewChignoli]

Addressed in #20. Kind of hard to follow since there are so many changes, but the fix happened when we made the loop constraint class