segmentation fault when use nlopt_optimize of NLopt in mjcb_control

[songdaegeun]

Hi, I'm a student and I'm using mujoco3.1.4. with nlopt 2.7.1.

In my code, inverse_kinematics() is called in callback "mycontroller".
then segmentation fault occur.

when I try to call mycontroller() in main(), It work well.

I don't understand why the result is different depending on where I call the callback function.
Is this a bug?

debug result

Process 90889 stopped
* thread #1, queue = 'com.apple.main-thread', stop reason = EXC_BAD_ACCESS (code=2, address=0x16f603d98)
    frame #0: 0x0000000100841aec libmujoco.3.1.4.dylib`mj_kinematics + 612
libmujoco.3.1.4.dylib`mj_kinematics:
->  0x100841aec <+612>: str    x19, [sp, #0x8]
    0x100841af0 <+616>: add    x8, sp, #0x50
    0x100841af4 <+620>: add    x21, x8, #0xd
    0x100841af8 <+624>: adrp   x0, 444
Target 0: (test) stopped.
(lldb) bt
* thread #1, queue = 'com.apple.main-thread', stop reason = EXC_BAD_ACCESS (code=2, address=0x16f603d98)
  * frame #0: 0x0000000100841aec libmujoco.3.1.4.dylib`mj_kinematics + 612
    frame #1: 0x0000000100961ac4 libmujoco.3.1.4.dylib`mj_fwdPosition + 136
    frame #2: 0x0000000100965bec libmujoco.3.1.4.dylib`mj_forwardSkip + 132
    frame #3: 0x0000000100002990 test`myequalityconstraints [inlined] simulator(Xin=<unavailable>, Xout=<unavailable>) at constrained.c:32:2 [opt]
    frame #4: 0x0000000100002950 test`myequalityconstraints(m=<unavailable>, result=0x000000011ea8bb80, n=<unavailable>, x=<unavailable>, grad=<unavailable>, equalitydata=<unavailable>) at constrained.c:5

test.c

#include<stdbool.h> //for bool
#include "mujoco/mujoco.h"
#include <GLFW/glfw3.h>
#include "stdio.h"
#include "stdlib.h"
#include "string.h"

#include "constrained.c"

double a = 0.25;
const double omega = 1;
double center_x = 1, center_z = 1.25;
double xref,zref,xact,zact;

// simulation end time
double simend = 2*3.14/omega+2;

//related to writing data to a file
FILE *fid;
int loop_index = 0;
const int data_chk_period = 50; //frequency at which data is written to a file

//Change the path 
char path[] = "./";
char datafile[] = "data.csv";
char xmlfile[] = "test.xml";

// MuJoCo data structures
mjModel* m = NULL;                  // MuJoCo model
mjData* d = NULL;                   // MuJoCo data
mjData* dsim = NULL; 
mjvCamera cam;                      // abstract camera
mjvOption opt;                      // visualization options
mjvScene scn;                       // abstract scene
mjrContext con;                     // custom GPU context

// mouse interaction
bool button_left = false;
bool button_middle = false;
bool button_right =  false;
double lastx = 0;
double lasty = 0;

// holders of one step history of time and position to calculate dertivatives
mjtNum position_history = 0;
mjtNum previous_time = 0;

// controller related variables
float_t ctrl_update_freq = 100;
mjtNum last_update = 0.0;
mjtNum ctrl;

// keyboard callback
void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods)
{
    // backspace: reset simulation
    if( act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE )
    {
        mj_resetData(m, d);
        mj_forward(m, d);
    }
}

// mouse button callback
void mouse_button(GLFWwindow* window, int button, int act, int mods)
{
    // update button state
    button_left =   (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS);
    button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS);
    button_right =  (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS);

    // update mouse position
    glfwGetCursorPos(window, &lastx, &lasty);
}


// mouse move callback
void mouse_move(GLFWwindow* window, double xpos, double ypos)
{
    // no buttons down: nothing to do
    if( !button_left && !button_middle && !button_right )
        return;

    // compute mouse displacement, save
    double dx = xpos - lastx;
    double dy = ypos - lasty;
    lastx = xpos;
    lasty = ypos;

    // get current window size
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // get shift key state
    bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||
                      glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);

    // determine action based on mouse button
    mjtMouse action;
    if( button_right )
        action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
    else if( button_left )
        action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
    else
        action = mjMOUSE_ZOOM;

    // move camera
    mjv_moveCamera(m, action, dx/width, dy/height, &scn, &cam);
}

// scroll callback
void scroll(GLFWwindow* window, double xoffset, double yoffset)
{
    // emulate vertical mouse motion = 5% of window height
    mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam);
}

// ****************************
//This function is called once and is used to get the headers
void init_save_data()
{
  //write name of the variable here (conventionally, "t," is header of time series data for csv file.)
   fprintf(fid,"t, ");
   fprintf(fid,"xref,zref,xact,zact\n");
   
   //Don't remove the newline
   fprintf(fid,"\n");
}

//***************************
//This function is called at a set frequency, put data here
void save_data(const mjModel* m, mjData* d)
{
  //data here should correspond to headers in init_save_data()
  //seperate data by a space %f followed by space
  fprintf(fid,"%f, ",d->time);
  fprintf(fid,"%f, %f, %f, %f\n", xref,zref,xact,zact);

  //Don't remove the newline
  fprintf(fid,"\n");
}

/******************************/
void set_torque_control(const mjModel* m,int actuator_no,int flag)
{
  if (flag==0)
    m->actuator_gainprm[10*actuator_no+0]=0;
  else
    m->actuator_gainprm[10*actuator_no+0]=1;
}
/******************************/


/******************************/
void set_position_servo(const mjModel* m,int actuator_no,double kp)
{
  m->actuator_gainprm[10*actuator_no+0]=kp;
  m->actuator_biasprm[10*actuator_no+1]=-kp;
}
/******************************/

/******************************/
void set_velocity_servo(const mjModel* m,int actuator_no,double kv)
{
  m->actuator_gainprm[10*actuator_no+0]=kv;
  m->actuator_biasprm[10*actuator_no+2]=-kv;
}
/******************************/

void curve(double t, double Xref[2]) {
	double denominator = 1 + sin(omega*t)*sin(omega*t);
	Xref[0] = center_x + a*cos(omega*t)/denominator;
	Xref[1] = center_z + a*sin(omega*t)*cos(omega*t)/denominator;
}

//**************************
void init_controller(const mjModel* m, mjData* d)
{
	double Xin[2] = {-0.5,1}; // joint angle 
	double Xref[2] = {0}; // target
	double Xout[2] = {0}; 
	simulator(Xin, Xout);
	center_x = Xout[0] - a;
	center_z = Xout[1];
	curve(0, Xref);
	inverse_kinematics(Xin,Xref);
	d->qpos[0] = Xin[0]; d->qpos[1] = Xin[1];            // set initial pose
	d->ctrl[0] = d->qpos[0]; d->ctrl[2] = d->qpos[1];    // set initial control
	mj_forward(m,d);

	xref=Xref[0]; zref=Xref[1];
	xact=d->sensordata[0]; zact=d->sensordata[2];
	// printf("qpos: %f %f\n",Xin[0],Xin[1]);
	// printf("tip: %f %f\n",d->sensordata[0],d->sensordata[2]);
}

//**************************
void mycontroller(const mjModel* m, mjData* d)
{
	// // write control here
	// double Xin[2] = {d->qpos[0], d->qpos[1]};  // initial pose를 현재 joint angle로 해놓으면 optimal pose가 근처에 있기때문에 IK계산이 수월하다.
	// printf("qpos:%f %f\n",d->qpos[0],d->qpos[1]);
	double Xin[2] = {-0.5,1};
	// double Xin[2] = {0,0};
	double Xref[2] = {0}; // target
	curve(d->time, Xref);

	// mycontroller가 콜백인데, 콜백내에서 nlopt_optimize를 쓰면안되나?
	inverse_kinematics(Xin,Xref);
	d->ctrl[0] = Xin[0]; d->ctrl[2] = Xin[1];
	mj_forward(m,d);

	xref=Xref[0]; zref=Xref[1];
	xact=d->sensordata[0]; zact=d->sensordata[2];
  //write data here (dont change/dete this function call; instead write what you need to save in save_data)
  if ( loop_index%data_chk_period==0)
    {
      save_data(m,d);
    }
  loop_index = loop_index + 1;
}

// main function
int main(int argc, const char** argv)
{
    char xmlpath[100]={};
    char datapath[100]={};

    strcat(xmlpath,path);
    strcat(xmlpath,xmlfile);

    strcat(datapath,path);
    strcat(datapath,datafile);

    // load and compile model
    char error[1000] = "Could not load binary model";

    // check command-line arguments
    if( argc<2 )
        m = mj_loadXML(xmlpath, 0, error, 1000);

    else
        if( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )
            m = mj_loadModel(argv[1], 0);
        else
            m = mj_loadXML(argv[1], 0, error, 1000);
    if( !m )
        mju_error_s("Load model error: %s", error);

    // make data
    d = mj_makeData(m);
	dsim = mj_makeData(m);

    // init GLFW
    if( !glfwInit() )
        mju_error("Could not initialize GLFW");

    // create window, make OpenGL context current, request v-sync
    GLFWwindow* window = glfwCreateWindow(1244, 700, "Demo", NULL, NULL);
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1);

    // initialize visualization data structures
    mjv_defaultCamera(&cam);
    mjv_defaultOption(&opt);
    mjv_defaultScene(&scn);
    mjr_defaultContext(&con);
    mjv_makeScene(m, &scn, 2000);                // space for 2000 objects
    mjr_makeContext(m, &con, mjFONTSCALE_150);   // model-specific context

    // install GLFW mouse and keyboard callbacks
    glfwSetKeyCallback(window, keyboard);
    glfwSetCursorPosCallback(window, mouse_move);
    glfwSetMouseButtonCallback(window, mouse_button);
    glfwSetScrollCallback(window, scroll);

	double arr_view[] = {90, -15, 7, 0.000000, 0.000000, 1.000000};
	cam.azimuth = arr_view[0];
	cam.elevation = arr_view[1];
	cam.distance = arr_view[2];
	cam.lookat[0] = arr_view[3];
	cam.lookat[1] = arr_view[4];
	cam.lookat[2] = arr_view[5];
	
	mjcb_control = mycontroller;

	fid = fopen(datapath,"w");
    init_save_data();
	init_controller(m,d);

    // use the first while condition if you want to simulate for a period.
    while( !glfwWindowShouldClose(window))
    {
        // advance interactive simulation for 1/60 sec
        // Assuming MuJoCo can simulate faster than real-time, which it usually can,
        // this loop will finish on time for the next frame to be rendered at 60 fps.
        // Otherwise add a cpu timer and exit this loop when it is time to render.
        mjtNum simstart = d->time;
        while(d->time - simstart < 1.0/60.0)
        {
            mj_step(m, d);
        }
		if (d->time>=simend)
		{
			fclose(fid);
			break;
		}
		// mycontroller(m,d);

       // get framebuffer viewport
        mjrRect viewport = {0, 0, 0, 0};
        glfwGetFramebufferSize(window, &viewport.width, &viewport.height);

		// opt.frame = mjFRAME_WORLD;
		// opt.flags[mjVIS_COM] = 1;
		// opt.flags[mjVIS_JOINT] = 1;

		// update scene and render
        mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
        mjr_render(viewport, &scn, &con);
        // printf("{%f, %f, %f, %f, %f, %f};\n",cam.azimuth,cam.elevation, cam.distance,cam.lookat[0],cam.lookat[1],cam.lookat[2]);

        // swap OpenGL buffers (blocking call due to v-sync)
        glfwSwapBuffers(window);

        // process pending GUI events, call GLFW callbacks
        glfwPollEvents();
    }

    // free visualization storage
    mjv_freeScene(&scn);
    mjr_freeContext(&con);

    // free MuJoCo model and data, deactivate
    mj_deleteData(d);
	mj_deleteData(dsim);
    mj_deleteModel(m);

    // terminate GLFW (crashes with Linux NVidia drivers)
    #if defined(__APPLE__) || defined(_WIN32)
        glfwTerminate();
    #endif

    return 1;
}

constrained.c

#include <stdio.h>
#include <math.h>

#include "nlopt.h"

/* optimization condition

cost = 0;

equality contrainits:
	ceq_1 = x_target - sensordata[0];
	ceq_2 = z_target - sensordata[2];

decision variables:
	Xin = {q_1, q_2}; joint angles

bounds:
	q_1, q_2 = {-3.14, 3.14};

*/

extern mjModel* m;
extern mjData* dsim;

double x_target;
double z_target;

void simulator(double Xin[2], double Xout[2]) {
	
	dsim->qpos[0] = Xin[0]; dsim->qpos[1] = Xin[1];
	dsim->ctrl[0] = dsim->qpos[0]; dsim->ctrl[2] = dsim->qpos[1];
	
	mj_forward(m,dsim);
	// mj_step(m,dsim);
	Xout[0] = dsim->sensordata[0];
	Xout[1] = dsim->sensordata[2];
	// mj_resetData(m,dsim);
}

double mycost(unsigned n, const double *x, double *grad, void *costdata)
{
    double cost = 0;

    return cost;
}

double myequalityconstraints(unsigned m, double *result, unsigned n,
                             const double *x,  double *grad,
                             void *equalitydata)
{
	double Xout[2] = {0};
	simulator(x,Xout);

	result[0] = x_target - Xout[0]; //dsim->sensordata[0];
	result[1] = z_target - Xout[1]; //dsim->sensordata[2];
 }

void inverse_kinematics(double Xin[2],double Xref[2])
{
int i;
nlopt_opt opt;

x_target = Xref[0]; z_target = Xref[1];

//establish sizes
unsigned n = 2; //number of decision variables
unsigned m_eq = 2; //number of equality constraints

//bounds for decision variables
double lb[] = { -3.14, -3.14 }; /* lower bounds */
double ub[] = { 3.14, 3.14 }; /* lower bounds */

//Set the algorithm and dimensionality
//L,G = global/local
//D,N = derivative / no derivative
opt = nlopt_create(NLOPT_LN_COBYLA, n); /* algorithm and dimensionality */

//Set the lower and upper bounds
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);

//Set up cost
// mycost_data costdata;
// for (i=0;i<n;i++)
//   costdata.a[i]=1;
nlopt_set_min_objective(opt, mycost, NULL);

//set up equality constraint
double tol_eq[]={1e-8,1e-8};
// myequalityconstraints_data equalitydata;
// equalitydata.ceq_1 = 5;
// equalitydata.ceq_2 = 2;
nlopt_add_equality_mconstraint(opt, m_eq, myequalityconstraints, NULL, tol_eq);

// double tol_in[]={1e-8};
// myinequalityconstraints_data inequalitydata;
// inequalitydata.cin_1 = 5;
// nlopt_add_inequality_mconstraint(opt, m_in, myinequalityconstraints,&inequalitydata, tol_in);

nlopt_set_xtol_rel(opt, 1e-4);
// double x[] = { 1, 1, 1, 2, 1 };  // initial guess
double minf; /* `*`the` `minimum` `objective` `value,` `upon` `return`*` */
double error;
if ((error = nlopt_optimize(opt, Xin, &minf)) < 0) {
    printf("nlopt failed![%f]: %f %f\n",error, Xin[0], Xin[1]);
}
else {
    printf("found minimum at f(%g,%g) = %0.10g\n", Xin[0], Xin[1], minf);
}

nlopt_destroy(opt);
}

test.xml

<mujoco>
  <option timestep="0.001" integrator="RK4" gravity="0 0 0">
	<flag energy="enable" contact="enable"/>
  </option>
  <visual>
		<headlight ambient=".5 .5 .5"/>
  </visual>
  <worldbody>	
    <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
	
	<body pos="0.5 0 1.25" euler="0 90 0">
		<geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/>
		<joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 -.5"/>
		<body pos="0 0.1 1" euler="0 0 0">
			<geom type="cylinder" size=".05 .5" rgba=".9 .9 .9 1" mass="1"/>
			<joint name="pin2" type="hinge" axis="0 -1 0" pos="0 0 -.5"/>
			<site name="tip" pos="0 0 .5" size="0.1"/>
		</body>	
	</body>
  </worldbody>
  <actuator>
	<position name="pservo1" joint="pin" kp="100" />
	<velocity name="vservo1" joint="pin" kv="10" />
	<position name="pservo2" joint="pin2" kp="100" />
	<velocity name="vservo2" joint="pin2" kv="10" />
  </actuator>
  <sensor>
	<framepos  objtype='site' objname='tip' />
	<framelinvel  objtype='site' objname='tip' />
  </sensor>

</mujoco>

[yuvaltassa]

Nothing jumps out as obviously wrong, but of course staring at code is not the way to fix this. I'd recommend building from source and using a debugger.

[songdaegeun]

Nothing jumps out as obviously wrong, but of course staring at code is not the way to fix this. I'd recommend building from source and using a debugger.

thank you for your help! It was due to an infinite recursive call in the callback function.