Skip to content

Modify the value of model.site_pos during simulation, the value of data.site_xpos does‘t update accordingly #5

@BowenFF

Description

@BowenFF

When the simulation is running, I want to modify the pose of a site. I use the following code:

int site_id = get_site_id(site_name);
    if (site_id != -1) {
        Eigen::VectorXd translation_vec = DQ_robotics::vec3(DQ_robotics::translation(pose));
        model_->site_pos[3*site_id+0] = translation_vec[0];
        model_->site_pos[3*site_id+1] = translation_vec[1];
        model_->site_pos[3*site_id+2] = translation_vec[2];

        DQ rotation_dq = DQ_robotics::rotation(pose);
        rotation_dq = DQ_robotics::normalize(rotation_dq);  
        
        model_->site_quat[4*site_id+0] = rotation_dq.q(0);  // w
        model_->site_quat[4*site_id+1] = rotation_dq.q(1);  // x
        model_->site_quat[4*site_id+2] = rotation_dq.q(2);  // y
        model_->site_quat[4*site_id+3] = rotation_dq.q(3);  // z
        
        mj_forward(model_, data_);

After that, I found that the value of model.site_pos was successfully updated, but the value of data.site_xpos did not change accordingly. Why is this happening?

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions