Cartesian end-effector control using site transmission.
- Added `refsite` attribute to actuators with `site` transmission. - Such actuators now have a well defined length and can be used for e.g., Cartesian end-effector control. - Added example models and documentation describing the new feature. PiperOrigin-RevId: 471561218 Change-Id: I538f09af9600d5c53992c4a9fed325648aab4fa3
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
834e8dd506
commit
46da1285af
@@ -21,12 +21,16 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "src/engine/engine_util_blas.h"
|
||||
#include "src/engine/engine_util_spatial.h"
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::ElementsAre;
|
||||
using ::testing::Pointwise;
|
||||
using ::testing::DoubleNear;
|
||||
using CoreSmoothTest = MujocoTest;
|
||||
|
||||
static std::vector<mjtNum> GetVector(const mjtNum* array, int length) {
|
||||
@@ -195,6 +199,48 @@ TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) {
|
||||
TestConnect(kModelFilePath);
|
||||
}
|
||||
|
||||
// --------------------------- site actuators ----------------------------------
|
||||
|
||||
// Test Cartesian position control using site transmission with refsite
|
||||
TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
|
||||
constexpr char kRefsitePath[] = "engine/testdata/refsite.xml";
|
||||
const std::string xml_path = GetTestDataFilePath(kRefsitePath);
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// set pose target in ctrl (3 positions, 3 rotations)
|
||||
mjtNum targetpos[] = {.01, .02, .03};
|
||||
mjtNum targetrot[] = {.1, .2, .3};
|
||||
mju_copy3(data->ctrl, targetpos);
|
||||
mju_copy3(data->ctrl+3, targetrot);
|
||||
|
||||
// step for 5 seconds
|
||||
while (data->time < 5) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// get site IDs
|
||||
int refsite_id = mj_name2id(model, mjOBJ_SITE, "reference");
|
||||
int site_id = mj_name2id(model, mjOBJ_SITE, "end_effector");
|
||||
|
||||
// check that position matches target to within 1e-5 length units
|
||||
double tol_pos = 1e-5;
|
||||
mjtNum relpos[3];
|
||||
mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id);
|
||||
EXPECT_THAT(relpos, Pointwise(DoubleNear(tol_pos), targetpos));
|
||||
|
||||
// check that orientation matches target to within 1e-3 radians
|
||||
double tol_rot = 1e-3;
|
||||
mjtNum site_xquat[4], refsite_xquat[4], relrot[3];
|
||||
mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id);
|
||||
mju_mat2Quat(site_xquat, data->site_xmat+9*site_id);
|
||||
mju_subQuat(relrot, site_xquat, refsite_xquat);
|
||||
EXPECT_THAT(relrot, Pointwise(DoubleNear(tol_rot), targetrot));
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
|
||||
// ------------------------ ellipsoid fluid model ------------------------------
|
||||
|
||||
|
||||
Vendored
+61
@@ -0,0 +1,61 @@
|
||||
<mujoco>
|
||||
<!--
|
||||
Adding a high fluid viscosity and using implicit integration for extra stabillity.
|
||||
Extra stabillity is required because the abstract "arm" model is not very realistic.
|
||||
(e.g. 4 consecutive ball joints is not a realistic kinematic design)
|
||||
-->
|
||||
<option viscosity="10" integrator="implicit">
|
||||
<flag gravity="disable" contact="disable"/>
|
||||
</option>
|
||||
|
||||
<statistic meansize=".05"/>
|
||||
|
||||
<default>
|
||||
<default class="translation">
|
||||
<position kp="100" ctrlrange="-.25 .25"/>
|
||||
</default>
|
||||
<default class="rotation">
|
||||
<!--
|
||||
Note that the rotational control range is purposefully limited to (-pi/2, pi/2) to avoid the
|
||||
documented instabillity near pi, which is due to taking quaternion differences.
|
||||
Increase this range to pi or bigger in order to see the instabillity.
|
||||
See here for more details https://mujoco.readthedocs.io/en/latest/XMLreference.html#actuator
|
||||
-->
|
||||
<position kp=".2" ctrlrange="-1.571 1.571"/>
|
||||
</default>
|
||||
<joint damping=".01" stiffness=".0001"/>
|
||||
<site type="box" size=".012 .012 .012" rgba=".7 .7 .8 1"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 2"/>
|
||||
<geom type="box" size=".25 .25 .01" pos="0 0 -.01"/>
|
||||
<site name="reference" pos="0 0 .25"/>
|
||||
<body name="arm" pos="-.25 .25 0">
|
||||
<joint type="ball"/>
|
||||
<geom type="box" size=".01" fromto="0 0 0 0 0 .25"/>
|
||||
<body pos="0 0 .25">
|
||||
<joint type="ball"/>
|
||||
<geom type="box" size=".01" fromto="0 0 0 .25 0 0"/>
|
||||
<body pos=".25 0 0">
|
||||
<joint type="ball"/>
|
||||
<geom type="box" size=".01" fromto="0 0 0 0 -.2 0"/>
|
||||
<body pos="0 -.2 0">
|
||||
<joint type="ball"/>
|
||||
<geom type="box" size=".01" fromto="0 0 0 0 -.05 0"/>
|
||||
<site name="end_effector" pos="0 -.05 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<position name="x" site="end_effector" refsite="reference" gear="1 0 0 0 0 0" class="translation"/>
|
||||
<position name="y" site="end_effector" refsite="reference" gear="0 1 0 0 0 0" class="translation"/>
|
||||
<position name="z" site="end_effector" refsite="reference" gear="0 0 1 0 0 0" class="translation"/>
|
||||
<position name="rx" site="end_effector" refsite="reference" gear="0 0 0 1 0 0" class="rotation"/>
|
||||
<position name="ry" site="end_effector" refsite="reference" gear="0 0 0 0 1 0" class="rotation"/>
|
||||
<position name="rz" site="end_effector" refsite="reference" gear="0 0 0 0 0 1" class="rotation"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user