Make factors and others type-agnostic (precursor to SphericalJoints)

Previously, many factors and other places in the code were depending on JointTyped which I think was not appropriate. I refactored these to be agnostic to joint type as an intermediate step to adding Spherical/Ball and Universal joints. I believe the code is now much cleaner and less hacky.

For example, TwistFactor was previously using JointTyped and calling the JointTyped version of transformTwistTo. The problem with this is that then any instance of TwistFactor has to be templated / type known at compile-time. However, the type kind of has to be determined at run-time, e.g. reading from an SDF you couldn't possibly know the types before runtime. Now, TwistFactor calls the Joint version of transformTwistTo (argument const Values &q_and_q_dot).

Tradeoffs

The upside is that this makes the code much more general and easier to maintain (IMO) and will make adding Spherical/Ball/other joints much easier. The downside is a potential runtime penalty due to runtime polymorphism (not benchmarked, but I would guess negligible).

Alternatives

An alternative would be to shift all the runtime polymorphism to the graph generation (e.g. DynamicsGraph, aFactors, vFactors, etc) but I believe this makes the code much more difficult to maintain since we would have tons of RTTI, e.g.

if (auto *joint_cast = dynamic_pointer_cast<ScrewJointBase>(joint)) {
  graph.emplace_shared<TwistFactor<ScrewJointBase>>(args);
} else if ... // for every joint type, and for every factor type

Alternatively, the factor creation functions could also be moved back into the Joint classes, but I think previously Frank strongly pushed for moving the factor creation functions out since the Link and Joint classes should be purely for defining the robot configuration and not have anything to do with factor graphs / gtsam.

Notes/dependencies

https://github.com/borglab/gtsam/pull/885 (Adjoint with Jacobian) needs to be merged in first. After https://github.com/borglab/gtsam/pull/884 merges, we can also optionally clean up some code (I left comments about "Due to quirks of OptionalJacobian, this is the cleanest way (Gerry)" prior to having created that PR, but with the PR then I think it can be cleaner), but that's low priority.

API / Breaking Changes

A few factors previously took the raw keys as arguments. I changed it to just taking in the timestamp instead, since they were already taking in the joint and the keys can be obtained from the joints + time. Applies to both c++ and python
Changed a lot of JointTyped arguments to just Joint arguments which lets us remove all the ugly boost::static_pointer_cast<const JointTyped> casts
Renamed JointLimitFactor to JointLimitDoubleFactor since it really is specific to just double-type joints and we will have to be adding new JointLimitFactors for other Joint types soon
Removed AdjointMapJacobianQ in favor of pose.Adjoint(twist, H_pose, H_twist) combined with pose = joint->poseOf(link, pose_H_q) and H_q = H_pose * pose_H_q.
Added a virtual function transformWrenchToTorqueVector so that TorqueFactor could be made type-agnostic. This now mirrors all the other functions e.g. transformTwistTo transformTwistAccelTo etc.
Changed some boost::optional<Matrix &> to OptionalJacobian<-1, -1>

All unit tests still passing of course, so if anyone has objections, add unit tests :)

borglab / GTDynamics