$\newcommand{\deriv}[2][]{\dfrac{\partial #1}{\partial #2}} \newcommand{\tderiv}[2][]{\tfrac{\partial #1}{\partial #2}} \newcommand{\fderiv}[2][]{\dfrac{d #1}{d #2}} \newcommand{\derivII}[3][]{\dfrac{\partial^2 #1}{\partial #2 \partial #3}} \newcommand{\tderivII}[3][]{\tfrac{\partial^2 #1}{\partial #2 \partial #3}} \newcommand{\derivIII}[4][]{\dfrac{\partial^3 #1}{\partial #2 \partial #3 \partial #4}} \newcommand{\tderivIII}[4][]{\tfrac{\partial^3 #1}{\partial #2 \partial #3 \partial #4}} \newcommand{\derivIV}[5][]{\dfrac{\partial^4 #1}{\partial #2 \partial #3 \partial #4 \partial #5}} \newcommand{\half}{\tfrac{1}{2}} % Macro for 1/2 \newcommand{\ud}{\mathrm{d}} \newcommand{\op}{\circ} \newcommand{\dq}{\dot{q}} \newcommand{\ddq}{\ddot{q}} \newcommand{\dt}{\Delta t}$

ROSMidpointVI - ROS Midpoint Variational Integrator

The ROSMidpointVI class wraps the trep.MidpointVI class to implement a variational integrator with a few extra features for use in a ROS environment. This class is a superset of the trep.MidpointVI class, so see the trep.MidpointVI documentation for a full listing of the class description.

Initializing the ROSMidpointVI class creates a rospy publisher which will automatically publish all trep frames to the /tf topic when step() is called.

class trep.ros.ROSMidpointVI(system, timestep, tolerance=1e-10, num_threads=None)

Create a new empty mechanical system. system is a valid System object that will be simulation.

timestep is a different requirement for the ROSMidpointVI class. This

sets the fixed timestep for the system simulation. The value is in seconds, ie. 0.1.

tolerance sets the desired tolerance of the root solver when

solving the DEL equation to advance the integrator.

MidpointVI makes use of multithreading to speed up the calculations. num_threads sets the number of threads used by this integrator. If num_threads is None, the integrator will use the number of available processors reported by Python’s multiprocessing module.

Simulation

ROSMidpointVI.step(u1=tuple(), k2=tuple(), max_iterations=200, lambda1_hint=None, q2_hint=None)

Step the integrator forward by one timestep . This advances the time and solves the DEL equation. The current state will become the previous state (ie, \(t_2 \Rightarrow t_1\), \(q_2 \Rightarrow q_1\), \(p_2 \Rightarrow p_1\)). The solution will be saved as the new state, available through t2, q2, and p2. lambda will be updated with the new constraint force, and u1 will be updated with the value of u1. The frames are also published to the /tf topic.

lambda1 and q2 can be specified to seed the root solving algorithm. If they are None, the previous values will be used.

The method returns the number of root solver iterations needed to find the solution.

Raises a ConvergenceError exception if the root solver cannot find a solution after max_iterations.

ROSMidpointVI.sleep()

Calls the rospy.Rate.sleep() method set to timestep of the ROSMidpointVI class. This method attempts to keep a loop at the specified frequency accounting for the time used by any operations during the loop.

Raises a rospy.ROSInterruptException exception if sleep is interrupted by shutdown.