mujoco_mpc
mujoco_mpc copied to clipboard
google-deepmind
Predictive control on small scale robot (fruitfly)
Hello, I am trying to get Fruitfly model working with model predictive control but I am constantly getting Rollout divergence at step error. I am using the following set of model patch as well as task description.
fruitfly.xml.patch
diff --git a/flybody/fruitfly.xml b/flybody/fruitfly.xml
index e92beb0..393b2f9 100644
--- a/flybody/fruitfly.xml
+++ b/flybody/fruitfly.xml
@@ -1,7 +1,7 @@
<mujoco model="fruitfly">
<compiler autolimits="true" angle="radian" meshdir="assets"/>
- <option timestep="0.0001" gravity="0 0 -981" density="0.00128" viscosity="0.000185" cone="elliptic"
+ <option timestep="0.001" gravity="0 0 -981" density="0.00128" viscosity="0.000185" cone="elliptic"
noslip_iterations="3"/>
<size njmax="300" nconmax="100" nkey="1"/>
@@ -882,7 +882,7 @@
<exclude name="rostrum_labrum_right" body1="rostrum" body2="labrum_right"/>
</contact>
- <tendon>
+ <!--tendon>
<fixed name="abduct_abdomen">
<joint joint="abdomen_abduct" coef="1"/>
<joint joint="abdomen_abduct_2" coef="1"/>
@@ -937,10 +937,10 @@
<joint joint="tarsus4_T3_right" coef="0.5"/>
<joint joint="tarsus5_T3_right" coef="0.5"/>
</fixed>
- </tendon>
+ </tendon-->
<actuator>
- <general name="head_abduct" class="head" ctrlrange="-0.2 0.2" joint="head_abduct"/>
+ <!--general name="head_abduct" class="head" ctrlrange="-0.2 0.2" joint="head_abduct"/>
<general name="head_twist" class="head" ctrlrange="-3 3" joint="head_twist"/>
<general name="head" class="head" ctrlrange="-0.5 0.3" joint="head"/>
<general name="rostrum" class="head" ctrlrange="-1.24 0.183" joint="rostrum"/>
@@ -953,14 +953,14 @@
<general name="antenna_left" class="antenna_extend" ctrlrange="-0.2 0.5" joint="antenna_left"/>
<general name="antenna_abduct_right" class="antenna_abduct" ctrlrange="-0.4 0.8" joint="antenna_abduct_right"/>
<general name="antenna_twist_right" class="antenna_twist" ctrlrange="-0.1 0.09" joint="antenna_twist_right"/>
- <general name="antenna_right" class="antenna_extend" ctrlrange="-0.2 0.5" joint="antenna_right"/>
+ <general name="antenna_right" class="antenna_extend" ctrlrange="-0.2 0.5" joint="antenna_right"/-->
<general name="wing_yaw_left" class="yaw" joint="wing_yaw_left"/>
<general name="wing_roll_left" class="roll" joint="wing_roll_left"/>
<general name="wing_pitch_left" class="pitch" joint="wing_pitch_left"/>
<general name="wing_yaw_right" class="yaw" joint="wing_yaw_right"/>
<general name="wing_roll_right" class="roll" joint="wing_roll_right"/>
<general name="wing_pitch_right" class="pitch" joint="wing_pitch_right"/>
- <general name="abdomen_abduct" class="abduct_abdomen" ctrlrange="-0.7 0.7" tendon="abduct_abdomen"/>
+ <!--general name="abdomen_abduct" class="abduct_abdomen" ctrlrange="-0.7 0.7" tendon="abduct_abdomen"/>
<general name="abdomen" class="extend_abdomen" ctrlrange="-1.05 0.7" tendon="abdomen"/>
<general name="coxa_abduct_T1_left" class="abduct_coxa_T1" joint="coxa_abduct_T1_left"/>
<general name="coxa_twist_T1_left" class="twist_coxa_T1" joint="coxa_twist_T1_left"/>
@@ -1017,10 +1017,10 @@
<adhesion name="adhere_claw_T2_left" class="adhesion_claw" body="claw_T2_left"/>
<adhesion name="adhere_claw_T2_right" class="adhesion_claw" body="claw_T2_right"/>
<adhesion name="adhere_claw_T3_left" class="adhesion_claw" body="claw_T3_left"/>
- <adhesion name="adhere_claw_T3_right" class="adhesion_claw" body="claw_T3_right"/>
+ <adhesion name="adhere_claw_T3_right" class="adhesion_claw" body="claw_T3_right"/-->
</actuator>
- <sensor>
+ <!--sensor>
<accelerometer name="accelerometer" site="thorax"/>
<gyro name="gyro" site="thorax"/>
<velocimeter name="velocimeter" site="thorax"/>
@@ -1036,5 +1036,5 @@
<touch name="touch_claw_T2_right" site="claw_T2_right"/>
<touch name="touch_claw_T3_left" site="claw_T3_left"/>
<touch name="touch_claw_T3_right" site="claw_T3_right"/>
- </sensor>
+ </sensor-->
</mujoco>
task.xml
<mujoco model="Fruitfly Flying">
<include file="../common.xml"/>
<!-- modified from: https://github.com/google-deepmind/mujoco_menagerie/tree/main/skydio_x2 -->
<include file="fruitfly_modified.xml"/>
<statistic extent="2" center="0.0 0.0 0.0"/>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" diffuse="0.5 0.5 0.5" specular="0.3 0.3 0.3" directional="true" castshadow="false"/>
<geom name="floor" size="0 0 0.1" pos="0 0 -0.25" type="plane" material="blue_grid"/>
<body name="goal" mocap="true" pos=".3 0 0.26">
<geom size="0.12" contype="0" conaffinity="0" rgba="0 1 0 .5" group="2"/>
</body>
</worldbody>
<sensor>
<!-- cost -->
<user name="Position" dim="3" user="0 25.0 0 50.0"/>
<user name="Lin. Vel." dim="3" user="0 1.25 0.0 5.0"/>
<user name="Ang. Vel." dim="3" user="0 1.25 0.0 5.0"/>
<user name="Control" dim="4" user="0 1.0e-3 0.0 1.0"/>
<user name="Orientation" dim="2" user="0 0.0 0.0 50.0"/>
<!-- trace -->
<framepos name="trace0" objtype="site" objname="thorax"/>
<!-- residual -->
<framepos name="position" objtype="body" objname="thorax"/>
<framequat name="orientation" objtype="body" objname="thorax"/>
<framelinvel name="linear_velocity" objtype="body" objname="thorax"/>
<frameangvel name="angular_velocity" objtype="body" objname="thorax"/>
</sensor>
</mujoco>
flybody.cc
#include "mjpc/tasks/flybody/flybody.h"
#include <string>
#include <mujoco/mujoco.h>
#include "mjpc/task.h"
#include "mjpc/utilities.h"
namespace mjpc {
std::string Flybody::XmlPath() const {
return GetModelPath("flybody/task.xml");
}
std::string Flybody::Name() const { return "Flybody"; }
// --------------- Residuals for quadrotor task ---------------
// Number of residuals: 4
// Residual (0): position - goal position
// Residual (1): linear velocity - goal linear velocity
// Residual (2): angular velocity - goal angular velocity
// Residual (3): control - hover control
// Number of parameters: 6
// Residuals are numbers that the MPC taretes
// ------------------------------------------------------------
void Flybody::ResidualFn::Residual(const mjModel* model, const mjData* data,
double* residuals) const {
// ---------- Residual (0) ----------
double* position = SensorByName(model, data, "position");
mju_sub(residuals, position, data->mocap_pos, 3); //distance from goal pos
// ---------- Residual (1) ----------
double* linear_velocity = SensorByName(model, data, "linear_velocity");
mju_copy(residuals + 3, linear_velocity, 3);
// ---------- Residual (2) ----------
double* angular_velocity = SensorByName(model, data, "angular_velocity");
mju_copy(residuals + 6, angular_velocity, 3);
// ---------- Residual (3) ----------
double thrust = (model->body_mass[0] + model->body_mass[1]) *
mju_norm3(model->opt.gravity) / model->nu;
for (int i = 0; i < model->nu; i++) {
residuals[9 + i] = data->ctrl[i] - thrust;
}
}
// ----- Transition for quadrotor task -----
void Flybody::TransitionLocked(mjModel* model, mjData* data) {
// set mode to GUI selection
if (mode > 0) {
current_mode_ = mode - 1;
} else {
// goal position
const double* goal_position = data->mocap_pos;
// system's position
double* position = SensorByName(model, data, "position");
// position error
double position_error[3];
mju_sub3(position_error, position, goal_position);
double position_error_norm = mju_norm3(position_error);
if (position_error_norm <= 5.0e-1) {
// update task state
current_mode_ += 1;
if (current_mode_ == model->nkey) {
current_mode_ = 0;
}
}
}
//std::printf("mocap_pos : %.2f", *(data->mocap_pos));
// set goal
//mju_copy3(data->mocap_pos, model->key_mpos + 3 * current_mode_);
//std::printf("mocap_pos : %.2f", *(data->mocap_pos));
//mju_copy4(data->mocap_quat, model->key_mquat + 4 * current_mode_);
}
} // namespace mjpc
fruitfly.h
#ifndef MJPC_TASKS_FLYBODY_FLYBODY_H_
#define MJPC_TASKS_FLYBODY_FLYBODY_H_
#include <memory>
#include <string>
#include <mujoco/mujoco.h>
#include "mjpc/task.h"
namespace mjpc {
class Flybody : public Task {
public:
std::string Name() const override;
std::string XmlPath() const override;
class ResidualFn : public mjpc::BaseResidualFn {
public:
ResidualFn(const ResidualFn& residual) = default;
explicit ResidualFn(const Flybody* task) : mjpc::BaseResidualFn(task) {}
// --------------- Residuals for quadrotor task ---------------
// Number of residuals: 5
// Residual (0): position - goal position
// Residual (1): orientation - goal orientation
// Residual (2): linear velocity - goal linear velocity
// Residual (3): angular velocity - goal angular velocity
// Residual (4): control
// Number of parameters: 6
// ------------------------------------------------------------
void Residual(const mjModel* model, const mjData* data,
double* residual) const override;
};
Flybody() : residual_(this) {}
void TransitionLocked(mjModel* model, mjData* data) override;
protected:
std::unique_ptr<mjpc::ResidualFn> ResidualLocked() const override {
return std::make_unique<ResidualFn>(this);
}
ResidualFn* InternalResidual() override { return &residual_; }
private:
int current_mode_ = 0;
ResidualFn residual_;
};
} // namespace mjpc
#endif // MJPC_TASKS_FLYBODY_FLYBODY_H_
Essentially I am removing all actuators that are not wing and using the same task class as the quadrotor. As for current setup, without setting the horizon to below 0.02 and setting time to 0.1, the rollout fails completely and the agent does not move at all. With small horizon, the fly moves but does not fly or make reasonable consistent progress towards the goal.
My question is, is there a parameter in the mjpc system that is used specifically for small robots with actuator that produces large force in proportion to its mass?
