def add_gripper(self, arm_name, gripper):
"""
Mounts gripper to arm.
Throws error if robot already has a gripper or gripper type is incorrect.
Args:
arm_name (str): name of arm mount
gripper (MujocoGripper instance): gripper MJCF model
"""
if arm_name in self.grippers:
raise ValueError("Attempts to add multiple grippers to one body")
arm_subtree = self.worldbody.find(
".//body[@name='{}']".format(arm_name))
for actuator in gripper.actuator:
if actuator.get("name") is None:
raise XMLError("Actuator has no name")
if not actuator.get("name").startswith("gripper"):
raise XMLError(
"Actuator name {} does not have prefix 'gripper'".format(
actuator.get("name")))
for body in gripper.worldbody:
arm_subtree.append(body)
self.merge(gripper, merge_body=False)
self.grippers[arm_name] = gripper
def merge(self, others, merge_body=True):
"""
Default merge method.
Args:
others (MujocoXML or list of MujocoXML): other xmls to merge into this one
raises XML error if @others is not a MujocoXML instance.
merges <worldbody/>, <actuator/> and <asset/> of @others into @self
merge_body (bool): True if merging child bodies of @others
Raises:
XMLError: [Invalid XML instance]
"""
if type(others) is not list:
others = [others]
for idx, other in enumerate(others):
if not isinstance(other, MujocoXML):
raise XMLError("{} is not a MujocoXML instance.".format(type(other)))
if merge_body:
for body in other.worldbody:
self.worldbody.append(body)
self.merge_asset(other)
for one_actuator in other.actuator:
self.actuator.append(one_actuator)
for one_sensor in other.sensor:
self.sensor.append(one_sensor)
for one_tendon in other.tendon:
self.tendon.append(one_tendon)
for one_equality in other.equality:
self.equality.append(one_equality)
for one_contact in other.contact:
self.contact.append(one_contact)
for one_default in other.default:
self.default.append(one_default)
def merge(self, other, merge_body=True):
"""
Default merge method.
Args:
other: another MujocoXML instance
raises XML error if @other is not a MujocoXML instance.
merges <worldbody/>, <actuator/> and <asset/> of @other into @self
merge_body: True if merging child bodies of @other. Defaults to True.
"""
if not isinstance(other, MujocoXML):
raise XMLError("{} is not a MujocoXML instance.".format(
type(other)))
if merge_body:
for body in other.worldbody:
self.worldbody.append(body)
self.merge_asset(other)
for one_actuator in other.actuator:
self.actuator.append(one_actuator)
for one_equality in other.equality:
self.equality.append(one_equality)
for one_contact in other.contact:
self.contact.append(one_contact)
for one_sensor in other.sensor:
self.sensor.append(one_sensor)
for one_default in other.default:
self.default.append(one_default)
def add_gripper(self, gripper, arm_name=None):
"""
Mounts gripper to arm.
Throws error if robot already has a gripper or gripper type is incorrect.
Args:
gripper (MujocoGripper): gripper MJCF model
arm_name (str): name of arm mount -- defaults to self.eef_name if not specified
Raises:
ValueError: [Multiple grippers]
XMLError: [No / invalid actuator]
"""
if arm_name is None:
arm_name = self.eef_name
if arm_name in self.grippers:
raise ValueError("Attempts to add multiple grippers to one body")
arm_subtree = self.worldbody.find(".//body[@name='{}']".format(arm_name))
for actuator in gripper.actuator:
if actuator.get("name") is None:
raise XMLError("Actuator has no name")
if not actuator.get("name").startswith("gripper"):
raise XMLError(
"Actuator name {} does not have prefix 'gripper'".format(
actuator.get("name")
)
)
for body in gripper.worldbody:
arm_subtree.append(body)
self.merge(gripper, merge_body=False)
self.grippers[arm_name] = gripper
# Update cameras in this model
self.cameras = self.get_element_names(self.worldbody, "camera")
def initialize_time(self, control_freq):
"""
Initializes the time constants used for simulation.
"""
self.cur_time = 0
self.model_timestep = self.sim.model.opt.timestep
if self.model_timestep <= 0:
raise XMLError("xml model defined non-positive time step")
self.control_freq = control_freq
if control_freq <= 0:
raise SimulationError(
"control frequency {} is invalid".format(control_freq))
self.control_timestep = 1. / control_freq
def initialize_time(self, control_freq):
"""
Initializes the time constants used for simulation.
Args:
control_freq (float): Hz rate to run control loop at within the simulation
"""
self.cur_time = 0
self.model_timestep = self.sim.timestep
if self.model_timestep <= 0:
raise XMLError("xml model defined non-positive time step")
self.control_freq = control_freq
if control_freq <= 0:
raise SimulationError(
"control frequency {} is invalid".format(control_freq))
self.control_timestep = 1. / control_freq
def merge(self, others, merge_body="default"):
"""
Default merge method.
Args:
others (MujocoXML or list of MujocoXML): other xmls to merge into this one
raises XML error if @others is not a MujocoXML instance.
merges <worldbody/>, <actuator/> and <asset/> of @others into @self
merge_body (None or str): If set, will merge child bodies of @others. Default is "default", which
corresponds to the root worldbody for this XML. Otherwise, should be an existing body name
that exists in this XML. None results in no merging of @other's bodies in its worldbody.
Raises:
XMLError: [Invalid XML instance]
"""
if type(others) is not list:
others = [others]
for idx, other in enumerate(others):
if not isinstance(other, MujocoXML):
raise XMLError("{} is not a MujocoXML instance.".format(
type(other)))
if merge_body is not None:
root = (self.worldbody if merge_body == "default" else
find_elements(root=self.worldbody,
tags="body",
attribs={"name": merge_body},
return_first=True))
for body in other.worldbody:
root.append(body)
self.merge_assets(other)
for one_actuator in other.actuator:
self.actuator.append(one_actuator)
for one_sensor in other.sensor:
self.sensor.append(one_sensor)
for one_tendon in other.tendon:
self.tendon.append(one_tendon)
for one_equality in other.equality:
self.equality.append(one_equality)
for one_contact in other.contact:
self.contact.append(one_contact)