Initialization at default_joint_position

[geraldb]

The simulation should initialize at the default_joint_position specified in the xml file, not at Zeros

[Samir Menon]

Most models have incorrect joint angle specs. Can't fix it till those are correct. Someone needs to check all the xml files first.

[Samir Menon]

Give it a shot and let me know!

[geraldb]

what do you mean with incorrect joint angles? the default values? i can just set all of them to zero if you like

[Samir Menon]

Please verify that all the joint limits and default joint positions are ok in the models in specs and send me fixes for the ones that aren't ok.

Once I'm sure that all the models work properly, I'll fix the code.

(PS: I can do that, but it is a very low pri so if you want it done fast, please send me the fix.).

[geraldb]

I looked through all the models and found a few cases that are either

x) definitely problematic (default < min or default > max)

x) possibly problematic, (default == min or default == max)

Do you want me to change them and send you the revised files? Here is the list:

DEFINITELY Problematic ==========================

PR2.xml:

    <link_name>head_tilt_link</link_name>
    <joint_limits>1.396260 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>head_pan_link</link_name>
    <joint_limits>3.007000 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>l_upper_arm_roll_link</link_name>
    <joint_limits>3.900000 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>
    <graphics>

    <link_name>l_shoulder_lift_link</link_name>
    <joint_limits>1.396300 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>l_shoulder_pan_link</link_name>
    <joint_limits>2.285400 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>r_upper_arm_roll_link</link_name>
    <joint_limits>0.800000 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>r_shoulder_lift_link</link_name>
    <joint_limits>1.396300 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>r_shoulder_pan_link</link_name>
    <joint_limits>0.714602 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

    <link_name>torso_lift_link</link_name>
    <joint_limits>0.310000 3.140000</joint_limits>
    <default_joint_position>0.0</default_joint_position>

Stanbot:

<link_name>right-lower-arm</link_name>
<joint_limits>-0.600000 3.140000</joint_limits>
<default_joint_position>-1.800000</default_joint_position>

<link_name>left-lower-arm</link_name>
<joint_limits>-0.600000 3.140000</joint_limits>
<default_joint_position>-1.800000</default_joint_position>

<link_name>right-upper-leg</link_name>
<joint_limits>-0.600000 3.140000</joint_limits>
<default_joint_position>-1.000000</default_joint_position>

POSSIBLY Problematic ==========================

RPP.xml:

    <link_name>link2</link_name>
    <joint_limits>-0.75 0.0</joint_limits>
    <default_joint_position>0.000000</default_joint_position>

PR2.xml:

    <link_name>l_wrist_flex_link</link_name>
    <joint_limits>0.000000 3.140000</joint_limits>
    <default_joint_position>0.000000</default_joint_position>

    <link_name>l_elbow_flex_link</link_name>
    <joint_limits>0.000000 3.140000</joint_limits>
    <default_joint_position>0.000000</default_joint_position>

    <link_name>r_wrist_flex_link</link_name>
    <joint_limits>0.000000 3.140000</joint_limits>
    <default_joint_position>0.000000</default_joint_position>

    <link_name>r_elbow_flex_link</link_name>
    <joint_limits>0.000000 3.140000</joint_limits>
    <default_joint_position>0.00</default_joint_position>

[geraldb]

Add this in CPostureApp::setInitialStateForUIAndDynamics() (there may be a more beautiful way to do this)

        // Initialize at defaut joint position
        int num_dof = robot_.getData()->io_data_->dof_;
        Eigen::VectorXd default_joint_pos( num_dof );

        for (int i=0; i < num_dof; ++i)
        {
            int link_id = robot_.getData()->parsed_robot_data_->robot_br_rep_.at(i)->link_id_;
            default_joint_pos( i ) = robot_.getData()->parsed_robot_data_->robot_br_rep_.at( link_id )->joint_default_pos_;
        }

[Samir Menon]

Confirmed. Will fix in next round of commits.

[Samir Menon]

Issue #98 was marked as a duplicate of this issue.

[Samir Menon]

Fixed. Though some robot specs aren't the greatest quality. Should be cleaned up.

[Samir Menon]

Resolved in upcoming version of scl.