Add SO3 transmission and native orientation actuator.
https://youtu.be/17XpwnqyCXs New transmission type mjTRN_SO3: a relative orientation, targeting a ball joint or a site+refsite pair. It is the first transmission with more than one force output: its length is the norm of the expmap vector of the relative rotation and its moment axes are the 3 rows of the relative rotational Jacobian, without projecting onto per-actuator gears. New force law mjGAIN_SO3/mjBIAS_SO3: a geodesic PD servo, force = kp * log(q_current^-1 * q_target) - kv * velocity, exact for arbitrary axis combinations with a unique equilibrium at every commanded orientation. Error, moment rows and velocity all live in the child frame (joint or site): the right-difference error is the gradient of the geodesic potential in that frame. The parent-frame (left) error is not: driving child-frame torques with it pumps energy at large angles, settling into steady-spinning limit cycles (the SO3LargeAngleConvergence test). The integrator variant stores the 3D orientation setpoint in act (actnum = 3, re-anchored to a bounded representative at integration time). Exposed in MJCF as <orientation joint=|site=+refsite= kp kv|dampratio>, or via <general gaintype="so3" biastype="so3">. The setpoint input has two charts: an expmap target (3 controls, default) or a quaternion target (4 controls) -- <orientation input="quat">, the first actuator with different input and output widths. The signature is recorded in a new per-actuator field actuator_ctrlspec (mjtCtrlChart), whose meaning is scoped by the gain type the way gain/bias parameters are; ctrlnum is derived from it at compile time and remains the layout authority. An explicit field rather than width inference or a prm slot: width-as-chart cannot express same-width signatures (upcoming servo input subsets), and prm slots are the input_mode pattern this stack retires. The force law normalizes the commanded quaternion, making it scale- and antipodally-invariant. The all-zero ctrl still maps to the identity via mju_normalize4, but it is a degenerate point (a nudge of any component commands a half-turn), so quat inputs reset to the identity quaternion: new mj_resetCtrl sets neutral ctrl values (zero, except qw = 1), called by mj_resetData and the viewers' Clear All. The quat chart is restricted to dyntype 'none': integrating a quaternion setpoint linearly is not meaningful on the manifold. New mjsActuator.ctrlspec field carries the signature through the spec and XML round-trip. Actuator sensors (actuatorpos/vel/frc) now report one value per force output; dim = 3 on an SO3 actuator. As the first actuator with nu != nactuator, this commit also makes the viewers multi-input aware: the control sliders in simulate and studio, which indexed per-actuator arrays by control index (out of bounds on this model class), are generated per control and labeled with the actuator name plus an input suffix ("orient/qw"), via the new introspection helper mj_actuatorInputName -- the single source of truth for input names, extended by each new multi-input type (quaternion components are w-first: qw, qx, qy, qz). Slider ranges now honor a defined ctrlrange even when ctrllimited is false: range is the UI hint, limited is the clamp -- wrapped and expmap setpoints are unbounded but still want finite sliders, while quat components are truly bounded. The rotational demo model is orientation.xml under test/engine/testdata/actuation/, upgraded to a three-way contrast: per-axis wrapped servos vs an expmap-commanded vs a quat-commanded orientation actuator, on identical checker-textured boxes. It is loaded by the mixed-axis contrast and input-name tests, and doubles as the viewer test model (slider groups of 3 independent, 3 grouped, 4 grouped). PiperOrigin-RevId: 951607063 Change-Id: If235dba8e2f2ca72672e7c62531a27e967c6a373
This commit is contained in:
committed by
Copybara-Service
parent
a8545ac7cc
commit
072e963fa0
@@ -1102,6 +1102,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
doc='Copy mjData, skip large arrays not required for visualization.',
|
||||
)),
|
||||
('mj_resetCtrl',
|
||||
FunctionDecl(
|
||||
name='mj_resetCtrl',
|
||||
return_type=ValueType(name='void'),
|
||||
parameters=(
|
||||
FunctionParameterDecl(
|
||||
name='m',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjModel', is_const=True),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='d',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjData'),
|
||||
),
|
||||
),
|
||||
),
|
||||
doc='Reset ctrl to neutral values: zero, except quaternion inputs which reset to the identity.', # pylint: disable=line-too-long
|
||||
)),
|
||||
('mj_resetData',
|
||||
FunctionDecl(
|
||||
name='mj_resetData',
|
||||
@@ -3467,6 +3487,30 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
doc='Get name of object with the specified mjtObj type and id; return NULL if name not found.', # pylint: disable=line-too-long
|
||||
)),
|
||||
('mj_actuatorInputName',
|
||||
FunctionDecl(
|
||||
name='mj_actuatorInputName',
|
||||
return_type=PointerType(
|
||||
inner_type=ValueType(name='char', is_const=True),
|
||||
),
|
||||
parameters=(
|
||||
FunctionParameterDecl(
|
||||
name='m',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjModel', is_const=True),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='id',
|
||||
type=ValueType(name='int'),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='input',
|
||||
type=ValueType(name='int'),
|
||||
),
|
||||
),
|
||||
doc='Get name of actuator input, determined by the actuator type and input signature; return NULL if the actuator type defines no input names.', # pylint: disable=line-too-long
|
||||
)),
|
||||
('mj_fullM',
|
||||
FunctionDecl(
|
||||
name='mj_fullM',
|
||||
@@ -10927,6 +10971,44 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
doc='Set actuator to velocity servo; return error if any.',
|
||||
)),
|
||||
('mjs_setToOrientation',
|
||||
FunctionDecl(
|
||||
name='mjs_setToOrientation',
|
||||
return_type=PointerType(
|
||||
inner_type=ValueType(name='char', is_const=True),
|
||||
),
|
||||
parameters=(
|
||||
FunctionParameterDecl(
|
||||
name='actuator',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjsActuator'),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='kp',
|
||||
type=ValueType(name='double'),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='kv',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(1,),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='dampratio',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(1,),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='ctrlspec',
|
||||
type=ValueType(name='int'),
|
||||
),
|
||||
),
|
||||
doc='Set actuator to orientation servo.',
|
||||
)),
|
||||
('mjs_setToDamper',
|
||||
FunctionDecl(
|
||||
name='mjs_setToDamper',
|
||||
|
||||
Reference in New Issue
Block a user