def testNamedJointsWithMultipleDOFs(self):
"""Regression test for b/77506142."""
physics = mujoco.Physics.from_xml_string(_MODEL_WITH_BALL_JOINTS_XML)
site_name = 'gripsite'
joint_names = ['joint_1', 'joint_2']
# This target position can only be achieved by rotating both ball joints. If
# DOFs are incorrectly indexed by joint index then only the first two DOFs
# in the first ball joint can change, and IK will fail to find a solution.
target_pos = (0.05, 0.05, 0)
result = ik.qpos_from_site_pose(physics=physics,
site_name=site_name,
target_pos=target_pos,
joint_names=joint_names,
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=True)
self.assertTrue(result.success)
self.assertLessEqual(result.steps, _MAX_STEPS)
self.assertLessEqual(result.err_norm, _TOL)
pos = physics.named.data.site_xpos[site_name]
np.testing.assert_array_almost_equal(pos, target_pos)
# IK should fail to converge if only the first joint is allowed to move.
physics.reset()
result = ik.qpos_from_site_pose(physics=physics,
site_name=site_name,
target_pos=target_pos,
joint_names=joint_names[:1],
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=True)
self.assertFalse(result.success)
def testNoTargetPosOrQuat(self):
physics = mujoco.Physics.from_xml_string(_ARM_XML)
site_name = 'gripsite'
with self.assertRaisesWithLiteralMatch(ValueError,
ik._REQUIRE_TARGET_POS_OR_QUAT):
ik.qpos_from_site_pose(physics=physics,
site_name=site_name,
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=True)
def testAllowedJointNameTypes(self, joint_names):
"""Test allowed types for joint_names parameter."""
physics = mujoco.Physics.from_xml_string(_ARM_XML)
site_name = 'gripsite'
target_pos = (0.05, 0.05, 0)
ik.qpos_from_site_pose(physics=physics,
site_name=site_name,
target_pos=target_pos,
joint_names=joint_names,
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=True)
def testDisallowedJointNameTypes(self, joint_names):
physics = mujoco.Physics.from_xml_string(_ARM_XML)
site_name = 'gripsite'
target_pos = (0.05, 0.05, 0)
expected_message = ik._INVALID_JOINT_NAMES_TYPE.format(
type(joint_names))
with self.assertRaisesWithLiteralMatch(ValueError, expected_message):
ik.qpos_from_site_pose(physics=physics,
site_name=site_name,
target_pos=target_pos,
joint_names=joint_names,
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=True)
def act(self, obs, env):
"""Returns the action that this controller would take at time t given observation obs.
Arguments:
obs: The current observation
t: The current timestep
get_pred_cost: If True, returns the predicted cost for the action sequence found by
the internal optimizer.
Returns: An action (and possibly the predicted cost)
"""
_TOL = 1e-14
_MAX_STEPS = 100
site_name = 'palm'
target_pos = env.physics.named.data.geom_xpos['target']
target_quat = env.physics.named.model.geom_quat['target']
# print('target pos: ', target_pos)
joint_names = [
'jaco_joint_1', 'jaco_joint_2', 'jaco_joint_3', 'jaco_joint_4',
'jaco_joint_5', 'jaco_joint_6'
]
result = ik.qpos_from_site_pose(physics=env.physics,
site_name=site_name,
target_pos=target_pos,
joint_names=joint_names,
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=False)
return result.qpos[0:9]
def _ik(self, pos):
out = inverse_kinematics.qpos_from_site_pose(
self.physics_copy,
'end_effector',
pos,
joint_names=('panda0_joint1', 'panda0_joint2', 'panda0_joint3',
'panda0_joint4', 'panda0_joint5', 'panda0_joint6'),
inplace=True)
return out.qpos[:]
def testQposFromSitePose(self, target, inplace, qpos_flag=False):
physics = mujoco.Physics.from_xml_string(FlexivPeg_XML)
target_pos, target_quat = target
count = 0
physics2 = physics.copy(share_model=True)
resetter = _ResetArm(seed=0)
while True:
result = ik.qpos_from_site_pose(
physics=physics2,
site_name=_SITE_NAME,
target_pos=target_pos,
target_quat=target_quat,
joint_names=_JOINTS,
tol=_TOL,
max_steps=_MAX_STEPS,
inplace=inplace,
)
if result.success:
break
elif count < _MAX_RESETS:
resetter(physics2)
count += 1
else:
raise RuntimeError(
'Failed to find a solution within %i attempts.' % _MAX_RESETS)
self.assertLessEqual(result.steps, _MAX_STEPS)
self.assertLessEqual(result.err_norm, _TOL)
# pdb.set_trace()
physics.data.qpos[:] = result.qpos
for i in range(len(self.sim.data.qpos)):
if qpos_flag:
self.sim.data.qpos[i]=physics.data.qpos[i]
else:
self.sim.data.ctrl[i] = physics.data.qpos[i]
# print(physics.data.qpos)
mjlib.mj_fwdPosition(physics.model.ptr, physics.data.ptr)
if target_pos is not None:
pos = physics.named.data.site_xpos[_SITE_NAME]
np.testing.assert_array_almost_equal(pos, target_pos)
if target_quat is not None:
xmat = physics.named.data.site_xmat[_SITE_NAME]
quat = np.empty_like(target_quat)
mjlib.mju_mat2Quat(quat, xmat)
quat /= quat.ptp() # Normalize xquat so that its max-min range is 1
def set_site_to_xpos(self,
physics,
random_state,
site,
target_pos,
target_quat=None,
max_ik_attempts=10):
"""Moves the arm so that a site occurs at the specified location.
This function runs the inverse kinematics solver to find a configuration
arm joints for which the pinch site occurs at the specified location in
Cartesian coordinates.
Args:
physics: A `mujoco.Physics` instance.
random_state: An `np.random.RandomState` instance.
site: Either a `mjcf.Element` or a string specifying the full name
of the site whose position is being set.
target_pos: The desired Cartesian location of the site.
target_quat: (optional) The desired orientation of the site, expressed
as a quaternion. If `None`, the default orientation is to point
vertically downwards.
max_ik_attempts: (optional) Maximum number of attempts to make at finding
a solution satisfying `target_pos` and `target_quat`. The joint
positions will be randomized after each unsuccessful attempt.
Returns:
A boolean indicating whether the desired configuration is obtained.
Raises:
ValueError: If site is neither a string nor an `mjcf.Element`.
"""
if isinstance(site, mjcf.Element):
site_name = site.full_identifier
elif isinstance(site, str):
site_name = site
else:
raise ValueError(
'site should either be a string or mjcf.Element: got {}'.
format(site))
if target_quat is None:
target_quat = DOWN_QUATERNION
lower, upper = self._get_joint_pos_sampling_bounds(physics)
arm_joint_names = [joint.full_identifier for joint in self.joints]
for _ in range(max_ik_attempts):
result = inverse_kinematics.qpos_from_site_pose(
physics=physics,
site_name=site_name,
target_pos=target_pos,
target_quat=target_quat,
joint_names=arm_joint_names,
rot_weight=2,
inplace=True)
success = result.success
# Canonicalise the angle to [0, 2*pi]
if success:
for arm_joint, low, high in zip(self.joints, lower, upper):
arm_joint_mj = physics.bind(arm_joint)
while arm_joint_mj.qpos >= high:
arm_joint_mj.qpos -= 2 * np.pi
while arm_joint_mj.qpos < low:
arm_joint_mj.qpos += 2 * np.pi
if arm_joint_mj.qpos > high:
success = False
break
# If succeeded or only one attempt, break and do not randomize joints.
if success or max_ik_attempts <= 1:
break
else:
self.randomize_arm_joints(physics, random_state)
return success
