Current Joint Configuration

[lukas-ramlab]

I'm trying to update only specific joints of a robot model, i.e., I have a vector with names and a corresponding vector with values.

Is there an easy way to do this?

I though of initializing a joint-vector with the current joint-position. How would I access this joint-position?

Thanks a lot for the help (and the great work on pinocchio).

[ManifoldFR]

Hello, thanks for your interest in Pinocchio!

You can do i = model.getJointId(name) to get the index of the joint in the list of joint models model.joints. To get the start index of the joint configuration in the robot configuration vector (typically denoted q), you can get it through model.idx_qs[i]. For the joint velocities similarly you can get the start index in the robot configuration velocity vector through model.idx_vs[i]. There are also attributes of the model object to get the corresponding joint configuration and velocity sizes.

[lukas-ramlab]

Thanks so much for the fast reply.

I tried to implement it, but I got stuck again. To access the joint data, should I use any of these two accessor-functions from the JointDataTpl:

ConfigVector_t 	joint_q () const
ConfigVector_t 	joint_q_accessor () const

[lukas-ramlab]

I came up with this implementation:

        Eigen::VectorXd joint_configuration(_model.nq);
        for (const pinocchio::JointModel &joint_model : _model.joints)
        {
            pinocchio::JointData joint_data = _data.joints[joint_model.id()];
            std::copy(joint_data.joint_q().begin(), joint_data.joint_q().end(), joint_configuration.begin() + joint_model.idx_q());
        }
        return joint_configuration;

However, at times leads to memory errors such as:

malloc(): unsorted double linked list corrupted
Segmentation fault (core dumped)