1. Body i+1 Body i-1 Body i Joint i Joint i+ 1 to tip (outward) to base (inward) Sensor Frame Frame A Body i+2 Joint i+ 2 Reference Frame Ground Frame root_body F Reference Frame Background: Mechanism_Model What? unified approach for modeling mechanical properties of robotic systems (mechanisms) model data stored in XML files each rigid body in the mechanism is modeled as a node in a tree at the root of the tree is a ground body that contains the inertial (ground) reference frame each body contains a joint and a reference frame; bodies may have additional local frames, geometric and dynamic properties implement generic kinematic (and dynamic) algorithms for commonly needed operations Why? provide centralized storage for managing kinematic (and dynamic) model information ensure consistency of the model information reduce duplication in model representation enable development of generic algorithms support specific implementations to override generic algorithms enable verification of specialized kinematics algorithms

2. Mechanism_Model: XML File Example <Mechanism_Model name = "r8_rover“ version = "1.0“ zero_position = "true"> <ME_Joint name = "traverse“ type = "full6dof“ home = "0.0" > <Quaternion qi="0“ qj="0“ qk="0” qs="1" /> <Position x = "0.0279“ y = "-0.0004“ z = "-0.1165" /> <Quaternion qi="0“ qj="0“ qk="0” qs="1" /> <Inertia xx = "1.1E+00“ xy = "-1.1E-02“ xz = "-1.1E-01" yx = "-1.1E-02“ yy = "2.0E+00“ yz = "8.6E-03" zx = "-1.1E-01“ zy = "8.6E-03“ zz = "2.6E+00" /> <Bounding_Shape name = "obj20“ is_container = “false“ is_aligned = "false“ ignore_collisions = "false“ tol = "-1.0" > <Quaternion qi="0“ qj="0“ qk="0” qs="1" />

3. Loading in a model: Mechanism_Model mm; Mechanism_Model_IO mm_io(mm); mm_io.load(“path/sampleR8_Rover.xml”); Saving model data to a file: mm_io.save(“path/sampleR8_Rover_copy.xml”); Querying for a frame location: mm.get_joint(i).set_value(joint_i_value); … cout << “The location of the left bogie is ” << mm.get_body("bogie_left").get_absolute_transform() << endl; Mechanism_Model: Examples

4. DescriptionInputOutputFeatures/ Assumptions Position-based, Open-loop, fixed radius arc, flat terrain length, headingsteer angles wheel drive distances Circular arc drive sequences, Flat terrain model cmd. length, heading, one wheel’s drive distance fraction of motion completed Velocity-based, Closed-loop, flat terrain desired body velocity & angular velocity Wheel steer angles, wheel velocities Instantaneous circular arc, Flat terrain model Position-based, Open-loop, variable radius arc, flat terrain final position (x, y) & heading wheel steer angles, wheel drive setpoints Clothoid arc sequences, Flat-terrain model Velocity-based, Closed-loop, rocker bogie kinematics, non-flat terrain desired body velocitywheel velocities, steer angles customized algorithms for rocker- bogie rovers, full 3-D kinematics, continuous pose updates wheel velocities, steer angles chassis velocity, angular velocity Review of CLARAty Locomotion Kinematic Algorithms

5. Proposed Wheel_Locomotor Classes

6. class Wheel_Locomotor_Model { public: enum WHEEL_LOCOMOTOR_TYPE {FULLY_STEERABLE, PARTIALLY_STEERABLE, SKID}; /** @{ @name Constructors/destructors */ Wheel_Locomotor_Model(Mechanism_Model & mm, ME_Body & rover_body); /** @} */ /** @{ @name Observers */ int get_number_of_wheels(); int get_number_of_steer_joints(); WHEEL_LOCOMOTOR_TYPE get_type(); /** @} */ /** @{ @name Kinematic algorithms */ virtual void compute_wheel_velocities ( double & linear_velocity, double & direction, double & angular_velocity, Vector & steer_angles, Vector & wheel_velocities); virtual void compute_body_velocities ( Vector & steer_angles, Vector & wheel_velocities, double & linear_velocity, double & direction, double & angular_velocity); virtual void compute_wheel_motion ( double & arclength, double & heading, Vector & steer_angles, Vector & wheel_distances); virtual void compute_body_motion ( Vector & steer_angles, Vector & wheel_distances, double & arclength, double & heading); Wheel_Locomotor_Model API

7. /** @} */ protected: virtual int _map_mechanism_model_to_wheel_locomotor_model(); virtual void _compute_jacobian(); Mechanism_Model & _mechanism_model; ME_Body & _rover_body; vector _wheels; int _number_of_steer_joints; WHEEL_LOCOMOTOR_TYPE _steer_type; Matrix _jacobian; Matrix _pseudoinverse_jacobian; private: }; Wheel_Locomotor_Model API – cont.

8. Wheel_Model API class Wheel_Model { public: enum WHEEL_TYPE { NON_STEERABLE_WHEEL, STEERABLE_WHEEL}; /** @{ @name Constructors/destructors */ Wheel_Model(ME_Body & wheel_body, ME_Body & rover_body); /** @} */ /** @{ @name Observers */ double get_radius(); WHEEL_TYPE get_type(); bool is_steerable(); Transform get_ground_contact(); ME_Body & get_wheel_body(); /** @} */ /** @{ @name Set parameters*/ void set_type(WHEEL_TYPE wheel_type); /** @} */ protected: virtual bool _compute_wheel_radius(); ME_Body & _wheel_body; ME_Body & _rover_body; double _radius; WHEEL_TYPE _wheel_type; private: };

9. Locomotor Control-flow Option 1 (velocity-based, closed-loop mode) Uses Wheel_Locomotor Model compute_wheel_velocities() wheel steer angles & drive velocities Pose Estimate uses Wheel_Locomotor Model compute_body_velocities()+ IMU + visual odometry … delta position & heading desired position & heading estimated position and heading cmd. chassis linear & angular velocity Motor Controller Mechanism / Terrain dynamics Mechanism motion Path Generator Science planner/ navigator Controller

10. Locomotor Control-flow Option 2 (clothoid-arc, flat-terrain, open-loop mode) Path Generator path parameters (Clothoid) Wheel motion parameters (Motion_Sequence) Trajectory Generator joint setpoints Pose Estimate uses Wheel_Locomotor Model compute_wheel_velocities() + IMU + visual odometry … Controller position & heading estimated position and heading cmd. relative pose Motor Controller Mechanism/ terrain dynamics Mechanism motion Motion_Sequence_Generator uses Wheel_Locomotor Model compute_wheel_velocities() Science planner/ navigator

11. Locomotor Control-flow Option 3 (circular-arc, flat-terrain, open-loop mode) Path Generator path parameters (Drive_Sequence) Wheel motion parameters (Motion_Sequence) Trajectory Generator joint setpoints Pose Estimate uses Wheel_Locomotor Model compute_body_motion() + IMU + visual odometry … Controller position & heading estimated position and heading cmd. relative pose Motor Controller Mechanism/ terrain dynamics Mechanism motion Motion_Sequence_Generator uses Wheel_Locomotor Model compute_wheel_motion() Science planner/ navigator