Skip to content

Commit

Permalink
feat: adds services to change equality constraints (#24)
Browse files Browse the repository at this point in the history 
* feat: added services to change equality constraints
* test: added tests for equality constraint services
---------

Co-authored-by: Julian Leichert <[email protected]>
Co-authored-by: David Leins <[email protected]>
  • Loading branch information
@acodeodyssey @DavidPL1
3 people authored Feb 6, 2024
1 parent 9265f4e commit 1148f48
Show file tree
Hide file tree
Showing 24 changed files with 1,368 additions and 1 deletion.
12 changes: 11 additions & 1 deletion mujoco_ros/include/mujoco_ros/mujoco_env.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,9 @@
#include <mujoco_ros_msgs/GetBodyState.h>
#include <mujoco_ros_msgs/SetGeomProperties.h>
#include <mujoco_ros_msgs/GetGeomProperties.h>
#include <mujoco_ros_msgs/EqualityConstraintParameters.h>
#include <mujoco_ros_msgs/GetEqualityConstraintParameters.h>
#include <mujoco_ros_msgs/SetEqualityConstraintParameters.h>
#include <mujoco_ros_msgs/SetGravity.h>
#include <mujoco_ros_msgs/GetGravity.h>

Expand All @@ -92,7 +95,8 @@ struct CollisionFunctionDefault
{
CollisionFunctionDefault(int geom_type1, int geom_type2, mjfCollision collision_cb)
: geom_type1_(geom_type1), geom_type2_(geom_type2), collision_cb_(collision_cb)
{}
{
}

int geom_type1_;
int geom_type2_;
Expand Down Expand Up @@ -321,6 +325,12 @@ class MujocoEnv
mujoco_ros_msgs::SetGeomProperties::Response &resp);
bool getGeomPropertiesCB(mujoco_ros_msgs::GetGeomProperties::Request &req,
mujoco_ros_msgs::GetGeomProperties::Response &resp);
bool setEqualityConstraintParametersArrayCB(mujoco_ros_msgs::SetEqualityConstraintParameters::Request &req,
mujoco_ros_msgs::SetEqualityConstraintParameters::Response &resp);
bool setEqualityConstraintParameters(const mujoco_ros_msgs::EqualityConstraintParameters &parameters);
bool getEqualityConstraintParametersArrayCB(mujoco_ros_msgs::GetEqualityConstraintParameters::Request &req,
mujoco_ros_msgs::GetEqualityConstraintParameters::Response &resp);
bool getEqualityConstraintParameters(mujoco_ros_msgs::EqualityConstraintParameters &parameters);
// Action calls
void onStepGoal(const mujoco_ros_msgs::StepGoalConstPtr &goal);

Expand Down
251 changes: 251 additions & 0 deletions mujoco_ros/src/callbacks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,12 @@ void MujocoEnv::setupServices()
service_servers_.emplace_back(nh_->advertiseService("set_geom_properties", &MujocoEnv::setGeomPropertiesCB, this));
service_servers_.emplace_back(nh_->advertiseService("get_geom_properties", &MujocoEnv::getGeomPropertiesCB, this));

service_servers_.emplace_back(
nh_->advertiseService("set_eq_constraint_parameters", &MujocoEnv::setEqualityConstraintParametersArrayCB, this));

service_servers_.emplace_back(
nh_->advertiseService("get_eq_constraint_parameters", &MujocoEnv::getEqualityConstraintParametersArrayCB, this));

service_servers_.emplace_back(nh_->advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>(
"load_initial_joint_states", [&](auto /*&req*/, auto /*&res*/) {
std::lock_guard<std::recursive_mutex> lock(physics_thread_mutex_);
Expand Down Expand Up @@ -632,4 +638,249 @@ bool MujocoEnv::getGeomPropertiesCB(mujoco_ros_msgs::GetGeomProperties::Request
return true;
}

bool MujocoEnv::setEqualityConstraintParameters(const mujoco_ros_msgs::EqualityConstraintParameters &parameters)
{
// look up equality constraint by name
ROS_DEBUG_STREAM("Looking up eqc by name '" << parameters.name << "'");
int eq_id = mj_name2id(model_.get(), mjOBJ_EQUALITY, parameters.name.c_str());
if (eq_id != -1) {
ROS_DEBUG_STREAM("Found eqc by name '" << parameters.name << "'");
int id1, id2;
switch (parameters.type.value) {
case mjEQ_TENDON:
id1 = mj_name2id(model_.get(), mjOBJ_TENDON, parameters.element1.c_str());
if (id1 != -1) {
model_->eq_obj1id[eq_id] = id1;
}
if (!parameters.element2.empty()) {
id2 = mj_name2id(model_.get(), mjOBJ_TENDON, parameters.element2.c_str());
if (id2 != -1) {
model_->eq_obj2id[eq_id] = id2;
}
}
model_->eq_data[eq_id * mjNEQDATA] = parameters.polycoef[0];
model_->eq_data[eq_id * mjNEQDATA + 1] = parameters.polycoef[1];
model_->eq_data[eq_id * mjNEQDATA + 2] = parameters.polycoef[2];
model_->eq_data[eq_id * mjNEQDATA + 3] = parameters.polycoef[3];
model_->eq_data[eq_id * mjNEQDATA + 4] = parameters.polycoef[4];
break;
case mjEQ_WELD:
id1 = mj_name2id(model_.get(), mjOBJ_XBODY, parameters.element1.c_str());
if (id1 != -1) {
model_->eq_obj1id[eq_id] = id1;
}
if (!parameters.element2.empty()) {
id2 = mj_name2id(model_.get(), mjOBJ_XBODY, parameters.element2.c_str());
if (id2 != -1) {
model_->eq_obj2id[eq_id] = id2;
}
}
model_->eq_data[eq_id * mjNEQDATA] = parameters.anchor.x;
model_->eq_data[eq_id * mjNEQDATA + 1] = parameters.anchor.y;
model_->eq_data[eq_id * mjNEQDATA + 2] = parameters.anchor.z;
model_->eq_data[eq_id * mjNEQDATA + 3] = parameters.relpose.position.x;
model_->eq_data[eq_id * mjNEQDATA + 4] = parameters.relpose.position.y;
model_->eq_data[eq_id * mjNEQDATA + 5] = parameters.relpose.position.z;
model_->eq_data[eq_id * mjNEQDATA + 6] = parameters.relpose.orientation.w;
model_->eq_data[eq_id * mjNEQDATA + 7] = parameters.relpose.orientation.x;
model_->eq_data[eq_id * mjNEQDATA + 8] = parameters.relpose.orientation.y;
model_->eq_data[eq_id * mjNEQDATA + 9] = parameters.relpose.orientation.z;
model_->eq_data[eq_id * mjNEQDATA + 10] = parameters.torquescale;
break;
case mjEQ_JOINT:
id1 = mj_name2id(model_.get(), mjOBJ_JOINT, parameters.element1.c_str());
if (id1 != -1) {
model_->eq_obj1id[eq_id] = id1;
}
if (!parameters.element2.empty()) {
id2 = mj_name2id(model_.get(), mjOBJ_JOINT, parameters.element2.c_str());
if (id2 != -1) {
model_->eq_obj2id[eq_id] = id2;
}
}
model_->eq_data[eq_id * mjNEQDATA] = parameters.polycoef[0];
model_->eq_data[eq_id * mjNEQDATA + 1] = parameters.polycoef[1];
model_->eq_data[eq_id * mjNEQDATA + 2] = parameters.polycoef[2];
model_->eq_data[eq_id * mjNEQDATA + 3] = parameters.polycoef[3];
model_->eq_data[eq_id * mjNEQDATA + 4] = parameters.polycoef[4];
break;
case mjEQ_CONNECT:
id1 = mj_name2id(model_.get(), mjOBJ_XBODY, parameters.element1.c_str());
if (id1 != -1) {
model_->eq_obj1id[eq_id] = id1;
}
if (!parameters.element2.empty()) {
id2 = mj_name2id(model_.get(), mjOBJ_XBODY, parameters.element2.c_str());

if (id2 != -1) {
model_->eq_obj2id[eq_id] = id2;
}
}
model_->eq_data[eq_id * mjNEQDATA] = parameters.anchor.x;
model_->eq_data[eq_id * mjNEQDATA + 1] = parameters.anchor.y;
model_->eq_data[eq_id * mjNEQDATA + 2] = parameters.anchor.z;
break;
default:
break;
}
model_->eq_active[eq_id] = parameters.active;
model_->eq_solimp[eq_id * mjNIMP] = parameters.solverParameters.dmin;
model_->eq_solimp[eq_id * mjNIMP + 1] = parameters.solverParameters.dmax;
model_->eq_solimp[eq_id * mjNIMP + 2] = parameters.solverParameters.width;
model_->eq_solimp[eq_id * mjNIMP + 3] = parameters.solverParameters.midpoint;
model_->eq_solimp[eq_id * mjNIMP + 4] = parameters.solverParameters.power;
model_->eq_solref[eq_id * mjNREF] = parameters.solverParameters.timeconst;
model_->eq_solref[eq_id * mjNREF + 1] = parameters.solverParameters.dampratio;
return true;
}
ROS_WARN_STREAM("Could not find specified equality constraint with name '" << parameters.name << "'");
return false;
}

bool MujocoEnv::setEqualityConstraintParametersArrayCB(mujoco_ros_msgs::SetEqualityConstraintParameters::Request &req,
mujoco_ros_msgs::SetEqualityConstraintParameters::Response &resp)
{
if (settings_.eval_mode) {
ROS_DEBUG("Evaluation mode is active. Checking hash validity");
if (settings_.admin_hash != req.admin_hash) {
ROS_ERROR("Hash mismatch, no permission to get geom properties!");
resp.status_message =
static_cast<decltype(resp.status_message)>("Hash mismatch, no permission to get geom properties!");
resp.success = false;
return true;
}
ROS_DEBUG("Hash valid, request authorized.");
}
resp.success = true;

bool failed_any = false;
bool succeeded_any = false;
for (const auto &parameters : req.parameters) {
bool success = setEqualityConstraintParameters(parameters);
failed_any = (failed_any || !success);
succeeded_any = (succeeded_any || success);
}

if (succeeded_any && failed_any) {
resp.status_message = static_cast<decltype(resp.status_message)>("Not all constraints could be set");
resp.success = false;
} else if (failed_any) {
resp.status_message = static_cast<decltype(resp.status_message)>("Could not set any constraints");
resp.success = false;
}

return true;
}

bool MujocoEnv::getEqualityConstraintParameters(mujoco_ros_msgs::EqualityConstraintParameters &parameters)
{
ROS_DEBUG_STREAM("Looking up Eq Constraint '" << parameters.name << "'");
// look up equality constraint by name
int eq_id = mj_name2id(model_.get(), mjOBJ_EQUALITY, parameters.name.c_str());
if (eq_id != -1) {
ROS_DEBUG("Found Eq Constraint");
parameters.type.value = model_->eq_type[eq_id];

std::vector<float> polycoef = std::vector<float>(5);

switch (model_->eq_type[eq_id]) {
case mjEQ_CONNECT:
parameters.element1 = mj_id2name(model_.get(), mjOBJ_JOINT, model_->eq_obj1id[eq_id]);
if (mj_id2name(model_.get(), mjOBJ_JOINT, model_->eq_obj2id[eq_id])) {
parameters.element2 = mj_id2name(model_.get(), mjOBJ_BODY, model_->eq_obj2id[eq_id]);
}
break;
case mjEQ_WELD:
parameters.element1 = mj_id2name(model_.get(), mjOBJ_BODY, model_->eq_obj1id[eq_id]);
if (mj_id2name(model_.get(), mjOBJ_BODY, model_->eq_obj2id[eq_id])) {
parameters.element2 = mj_id2name(model_.get(), mjOBJ_BODY, model_->eq_obj2id[eq_id]);
}
parameters.anchor.x = model_->eq_data[eq_id * mjNEQDATA];
parameters.anchor.y = model_->eq_data[eq_id * mjNEQDATA + 1];
parameters.anchor.z = model_->eq_data[eq_id * mjNEQDATA + 2];
parameters.relpose.position.x = model_->eq_data[eq_id * mjNEQDATA + 3];
parameters.relpose.position.y = model_->eq_data[eq_id * mjNEQDATA + 4];
parameters.relpose.position.z = model_->eq_data[eq_id * mjNEQDATA + 5];
parameters.relpose.orientation.w = model_->eq_data[eq_id * mjNEQDATA + 6];
parameters.relpose.orientation.x = model_->eq_data[eq_id * mjNEQDATA + 7];
parameters.relpose.orientation.y = model_->eq_data[eq_id * mjNEQDATA + 8];
parameters.relpose.orientation.z = model_->eq_data[eq_id * mjNEQDATA + 9];
parameters.torquescale = model_->eq_data[eq_id * mjNEQDATA + 10];
break;
case mjEQ_JOINT:
parameters.element1 = mj_id2name(model_.get(), mjOBJ_JOINT, model_->eq_obj1id[eq_id]);
if (mj_id2name(model_.get(), mjOBJ_JOINT, model_->eq_obj2id[eq_id])) {
parameters.element2 = mj_id2name(model_.get(), mjOBJ_JOINT, model_->eq_obj2id[eq_id]);
}
parameters.polycoef = { model_->eq_data[eq_id * mjNEQDATA], model_->eq_data[eq_id * mjNEQDATA + 1],
model_->eq_data[eq_id * mjNEQDATA + 2], model_->eq_data[eq_id * mjNEQDATA + 3],
model_->eq_data[eq_id * mjNEQDATA + 4] };
break;
case mjEQ_TENDON:
parameters.element1 = mj_id2name(model_.get(), mjOBJ_TENDON, model_->eq_obj1id[eq_id]);
if (mj_id2name(model_.get(), mjOBJ_TENDON, model_->eq_obj2id[eq_id])) {
parameters.element2 = mj_id2name(model_.get(), mjOBJ_TENDON, model_->eq_obj2id[eq_id]);
}
parameters.polycoef = { model_->eq_data[eq_id * mjNEQDATA], model_->eq_data[eq_id * mjNEQDATA + 1],
model_->eq_data[eq_id * mjNEQDATA + 2], model_->eq_data[eq_id * mjNEQDATA + 3],
model_->eq_data[eq_id * mjNEQDATA + 4] };
break;
default:
break;
}
parameters.active = model_->eq_active[eq_id];
parameters.solverParameters.dmin = model_->eq_solimp[eq_id * mjNIMP];
parameters.solverParameters.dmax = model_->eq_solimp[eq_id * mjNIMP + 1];
parameters.solverParameters.width = model_->eq_solimp[eq_id * mjNIMP + 2];
parameters.solverParameters.midpoint = model_->eq_solimp[eq_id * mjNIMP + 3];
parameters.solverParameters.power = model_->eq_solimp[eq_id * mjNIMP + 4];
parameters.solverParameters.timeconst = model_->eq_solref[eq_id * mjNREF];
parameters.solverParameters.dampratio = model_->eq_solref[eq_id * mjNREF + 1];
return true;
}
ROS_WARN_STREAM("Could not find equality constraint named '" << parameters.name << "'");
return false;
}

bool MujocoEnv::getEqualityConstraintParametersArrayCB(mujoco_ros_msgs::GetEqualityConstraintParameters::Request &req,
mujoco_ros_msgs::GetEqualityConstraintParameters::Response &resp)
{
if (settings_.eval_mode) {
ROS_DEBUG("Evaluation mode is active. Checking hash validity");
if (settings_.admin_hash != req.admin_hash) {
ROS_ERROR("Hash mismatch, no permission to get geom properties!");
resp.status_message =
static_cast<decltype(resp.status_message)>("Hash mismatch, no permission to get geom properties!");
resp.success = false;
return true;
}
ROS_DEBUG("Hash valid, request authorized.");
}
resp.success = true;

bool failed_any = false;
bool succeeded_any = false;
for (const auto &name : req.names) {
mujoco_ros_msgs::EqualityConstraintParameters eqc;
eqc.name = name;
bool success = getEqualityConstraintParameters(eqc);

failed_any = (failed_any || !success);
succeeded_any = (succeeded_any || success);
if (success) {
resp.parameters.emplace_back(eqc);
}
}

if (succeeded_any && failed_any) {
resp.status_message = static_cast<decltype(resp.status_message)>("Not all constraints could be fetched");
resp.success = false;
} else if (failed_any) {
resp.status_message = static_cast<decltype(resp.status_message)>("Could not fetch any constraints");
resp.success = false;
}

return true;
}

} // namespace mujoco_ros
67 changes: 67 additions & 0 deletions mujoco_ros/test/equality_world.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
<mujoco model="3DOF">
<option collision="all" timestep="0.001" gravity="0 0 -9.81" cone="elliptic" />
<compiler angle="radian" />

<visual>
<headlight ambient="0.4 0.4 0.4" diffuse="0.4 0.4 0.4" specular="0.0 0.0 0.0" active="1" />
</visual>

<asset>
<texture builtin="checker" height="512" name="texplane" rgb1=".2 .3 .4" rgb2=".1 .15 .2" type="2d" width="512" />
<material name="MatPlane" reflectance="0.5" shininess="0.01" specular="0.1" texrepeat="1 1" texture="texplane" texuniform="true" />
</asset>

<worldbody>
<light pos="0 0 1000" castshadow="false" />
<geom name="ground_plane" type="plane" size="5 5 10" material="MatPlane" rgba="1 1 1 1"/>

<body name="base_link">
<geom type="capsule" fromto="0 0 1 0 0 0.6" size="0.06"/>
<joint name="balljoint" type="ball" pos="0 0 1"/>
<body name="middle_link">
<geom type="capsule" fromto="0 0 0.6 0.0 0 0.3" size="0.04"/>
<joint name="joint1" type="hinge" pos="0 0 0.6" axis="0 1 0"/>
<!-- <joint name="joint2" type="hinge" pos="0 0 0.6" axis="1 0 0"/> -->
<body name="end_link">
<geom name="EE" type="capsule" fromto="0 0 0.3 0.0 0 0.1" size="0.02"/>
<joint name="joint2" type="hinge" pos="0.0 0 0.3" axis="0 1 0"/>
</body>
</body>
</body>
<body name="body_ball" pos="1 0 0.06" >
<freejoint name="ball_freejoint"/>
<geom name="ball" type="sphere" size="0.05" rgba="1.0 0.55 0.0 0.2" mass="0.1"/>
</body>
<body name="immovable" pos="0.56428 0.221972 0.6">
<geom type="box" size=".0125 .016 .032" rgba=".5 .5 .5 1" />
<inertial pos="0 0 0" mass="0.1024" diaginertia="4.369e-5 4.028e-5 1.407e-5"/>
<joint name="joint_eq_element1" type="hinge" pos="0 0 0.6" axis="0 1 0"/>
</body>
<body name="connect_eq_element1" pos="2 2 0.6">
<geom type="box" size=".0125 .016 .032" rgba=".5 .5 .5 1" />
<inertial pos="0 0 0" mass="0.1024" diaginertia="4.369e-5 4.028e-5 1.407e-5"/>
<joint name="joint_eq_element2" type="hinge" pos="0 0 0.6" axis="0 1 0"/>
</body>
</worldbody>
<tendon>
<fixed name="tendon_eq_element1" limited="true" range="-0.3 2">
<joint joint="joint_eq_element1" coef=".5"/>
</fixed>
<fixed name="tendon_eq_element2" limited="true" range="-0.3 2">
<joint joint="joint_eq_element2" coef=".5"/>
</fixed>
</tendon>
<equality>
<weld name="weld_eq" body1="immovable" torquescale="0.9" relpose="1.1 1.2 1.3 0.358 -0.003 -0.886 0.295" solref="0.3 0.9" solimp="0.8 0.95 0.002 0.4 2" active="true"/>
</equality>
<equality>
<joint name="joint_eq" joint1="joint_eq_element1" joint2="joint_eq_element2" polycoef="0.5 0.25 0.76 0.66 1" solref="0.3 0.9" solimp="0.8 0.95 0.002 0.4 2" active="true"/>
</equality>
<equality>
<tendon name="tendon_eq" tendon1="tendon_eq_element1" tendon2="tendon_eq_element2" polycoef="0.5 0.25 0.76 0.66 1" solref="0.3 0.9" solimp="0.8 0.95 0.002 0.4 2" active="true"/>
</equality>
<equality>
<connect anchor="0 0 0" name="connect_eq" body1="immovable" solref="0.3 0.9" solimp="0.8 0.95 0.002 0.4 1" active="true"/>
</equality>

</mujoco>
Loading

0 comments on commit 1148f48

Please sign in to comment.