scenarioDeployPanels
It’s recommended to review the following scenario(s) first (and any recommended scenario(s) that they may have):
examples/mujoco/scenarioArmWithThrusters.py
This script demonstrates how to simulate a spacecraft with solar panels deployed using a Proportional-Integral-Derivative (PID) controller. This script uses the MuJoCo-based DynamicObject MJScene.
In mujoco/scenarioArmWithThrusters.py
, we saw how we can constrain joints to
follow a specific angle by letting the dynamic engine figure out and apply
the necessary torques. In this script, we are controlling the joints using a
PID controller. This is a more adequate simulation setup when you want to simulate
or design the control system for these joints. It is also generally more
computationally efficient than letting the dynamic engine figure out the torques.
The multi-body system is defined in the XML file sat_w_deployable_panels.xml
.
This XML file defines a ‘hub’ body and six deployable solar panels. The panels
are attached to the ‘hub’ body via revolute joints, allowing them to rotate
from a stowed position to a deployed position.
This XML file (and this script) demonstrates the use of the range
attribute
in the joint
element. This attribute allows the joint to be limited to a
specific range of motion. For example, between 0 and 90 or 180 degrees. If the
joint tries to move beyond this range, the dynamic engine will apply a force to
prevent it from doing so (akin to a physical stop on the system). Note in the
plots and in the 3D visualization how the panels never get deployed over their
joint limit.
The deployment of the panels is controlled using an analog PID controller.
The desired position and velocity profiles for the joints are
generated using a trapezoidal/triangular velocity profile. These profiles are
then used as inputs to the PID controller, which computes the torque required to achieve
the desired motion. Note that the controller class extends StatefulSysModel
,
instead of SysModel
, since we need to register the integral error as a
continuous state.
The simulation is run for 80 minutes and the state of the system is recorded.
The desired and achieved joint angles, as well as the torque applied to each
joint, are plotted. The system can also be visualized in a 3D environment using
the mujoco.visualize
function.
- class scenarioDeployPanels.PIDController(*a, **kw)[source]
Bases:
StatefulSysModel
A Proportional-Integral-Derivative (PID) Controller class for controlling joint states.
This models an analog PID controller, which means that its output evolves in continuous time, not discrete time. Thus, it should be used within the dynamics task of
MJScene
.- K_p
Proportional gain.
- Type:
float
- K_d
Derivative gain.
- Type:
float
- K_i
Integral gain.
- Type:
float
- measuredInMsg
Reader for the measured joint state.
- Type:
messaging.ScalarJointStateMsgReader
- desiredInMsg
Reader for the desired joint state.
- Type:
messaging.ScalarJointStateMsgReader
- measuredDotInMsg
Reader for the measured joint state derivative.
- Type:
messaging.ScalarJointStateMsgReader
- desiredDotInMsg
Reader for the desired joint state derivative.
- Type:
messaging.ScalarJointStateMsgReader
- outputOutMsg
Output message, contains the torque for the connected actuator.
- Type:
messaging.SingleActuatorMsg
- scenarioDeployPanels.generateProfiles(initialPoint: float, finalPoint: float, vMax: float, aMax: float) List[ScalarJointStateInterpolator] [source]
Generate a position and velocity profile for a point-to-point movement.
This function calculates the position and velocity profiles for a point-to-point movement using either a trapezoidal or triangular velocity profile, depending on whether the maximum velocity is reached or not.
- Parameters:
initialPoint (float) – The starting position.
finalPoint (float) – The target position.
vMax (float) – The maximum velocity.
aMax (float) – The maximum acceleration.
- Returns:
- A list containing two mujoco.ScalarJointStateInterpolator objects,
one for the position profile and one for the velocity profile.
- Return type:
list
- scenarioDeployPanels.run(initialSpin: bool = False, showPlots: bool = False, visualize: bool = False)[source]
Main function, see scenario description.
- Parameters:
initialSpin (bool, optional) – If True, the satellite is given an initial angular velocity. Defaults to False.
showPlots (bool, optional) – If True, simulation results are plotted and show. Defaults to False.
visualize (bool, optional) – If True, the
MJScene
visualization tool is run on the simulation results. Defaults to False.