Humanoid controller based on visual info

Because of a standard Jacobian matrix assumed under the same angular velocity in each joints, humanoid robot perform a lean back movement when approaching the object. We want to adjust the movement of each joint to be more stable and natural, and apply a weighted pseudo-inverse matrix considering a weight value for each joint to standard Jacobian matrix in the update function in the Inverse Kinematics.
To adapt the environment changes, we measure the AR marker using HRP-4’s head USB camera. HRP-4’s dual-arm is controlled to the two AR marker in the robot coordinate system.