def __init__(self,
config,
use_visualizer=False,
name=None):
"""Initialize.
Args:
config (str, YamlConfig): A relative filepath to the config file. Or a parsed YamlConfig file as a dictionary.
e.g. cfg/my_config.yaml
use_visualizer (bool): A flag for whether or not to use visualizer
name (str): of the environment
Notes:
See documentation for more details about config files.
"""
# Get config dictionary.
if isinstance(config, YamlConfig):
self.config = config
else:
self.config = YamlConfig(config)
self.world = God.make_world(self.config,
use_visualizer,
name)
self.initialize()
def __init__(self, config, controlType):
"""
Initialize robot for control.
"""
try:
self.config = YamlConfig(config)
except TypeError:
self.config = config
# Timing
self.startTime = time.time()
self.endTime = time.time()
self.action_set = False
# Control Init
self.controlType = controlType
self.model = Model(offset_mass_matrix=True)
def __init__(self, config_file='cfg/ros_sensors.yaml', invert=True):
"""Initialize publisher
"""
self.config = YamlConfig(config_file)
# Override default invert flag with config setting.
if 'invert' in self.config:
self.invert = self.config['invert']
print("Invert is {}".format(self.invert))
else:
self.invert = invert
redis_kwargs = self.config['workstation_redis']
self.redisClient = RedisInterface(**redis_kwargs)
self.rgb_topics = self.config['rgb_topics']
self.depth_topics = self.config['depth_topics']
self.rgb_callbacks = {}
self.rgb_subscribers = {}
self.depth_callbacks = {}
self.msg_callbacks = {}
self.run()
def __init__(self,
ctrl_type,
demo_type,
test_fn,
config_file="demos/demo_control_cfg.yaml",
**kwargs):
self.ctrl_type = ctrl_type
self.config_file = config_file
# Overwrite config file controlType
self.config = YamlConfig(self.config_file)
self.config['world']['controlType'] = self.ctrl_type
# Use command line argument over the config spec.
if kwargs['world'] is not None:
self.config['world']['type'] = kwargs['world']
self.demo_type = demo_type
self.test_fn = test_fn
self.plot_pos = kwargs['plot_pos']
self.plot_error = kwargs['plot_error']
self.save_fig = kwargs['save_fig']
self.save = kwargs['save']
self.demo_name = kwargs['demo_name']
if self.demo_name is None:
now = datetime.now()
demo_name = now.strftime("%d%m%y%H%M%S")
self.demo_name = "{}_{}_{}".format(demo_name, self.ctrl_type,
self.demo_type)
# self.axis = kwargs['axis']
# Initialize lists for storing data.
self.errors = []
self.actions = []
self.states = []
self.observations = []
def __init__(self,
config='examples/safenet/safenet_example.yaml',
use_visualizer=True,
name="SafenetBoundaryEnv",
world_type=None,
safenet_config=None):
"""Initialize the environment.
Set up any variables that are necessary for the environment and your task.
"""
self.config = YamlConfig(config)
# Override world param in config file if arg passed.
if world_type is not None:
self.config['world']['type'] = world_type
if safenet_config is not None:
self.config['safenet'] = safenet_config['safenet']
super().__init__(self.config, use_visualizer, name)
if self.world.is_sim:
self.visualize_boundaries()
# Set to gravity compensation mode.
self.robot_interface.change_controller('JointTorque')
self.MAX_STEPS = 10000
def __init__(self,
config='cfg/sawyer_ctrl_config.yaml',
controlType='EEImpedance',
use_safenet=False,
node_name='sawyer_interface'):
"""
Initialize Sawyer Robot for control
Args:
use_safenet: bool
whether to use the Safenet to constrain ee positions
and velocities
node_name: str
name of the node for ROS
Attributes:
ee_position
ee_orientation
"""
self.log_start_times = []
self.log_end_times = []
self.cmd_start_times = []
self.cmd_end_times = []
# Connect to redis client
# use default port 6379 at local host.
self.config = YamlConfig(config)
self.redisClient = RobotRedis(**self.config['redis'])
self.redisClient.flushall()
# Timing
self.startTime = time.time()
self.endTime = time.time()
self.action_set = False
self.model = Model()
self.ee_name = self.config['sawyer']['end_effector_name']
if config is not None:
if self.config['controller']['interpolator']['type'] == 'linear':
interp_kwargs = {
'max_dx': 0.005,
'ndim': 3,
'controller_freq': 500,
'policy_freq': 20,
'ramp_ratio': 0.2
}
self.interpolator_pos = LinearInterpolator(**interp_kwargs)
self.interpolator_ori = LinearOriInterpolator(**interp_kwargs)
rospy.loginfo(
"Linear interpolator created with params {}".format(
interp_kwargs))
else:
self.interpolator_pos = None
self.interpolator_ori = None
self.interpolator_goal_set = False
start = time.time()
rospy.loginfo('Initializing Sawyer robot interface...')
try:
self._head = iif.Head()
self._display = iif.HeadDisplay()
self._lights = iif.Lights()
self._has_head = True
except:
rospy.logerr('No head found, head functionality disabled')
self._has_head = False
self._limb = iif.Limb(limb="right", synchronous_pub=False)
self._joint_names = self._limb.joint_names()
self.cmd = []
try:
self._gripper = iif.Gripper()
self._has_gripper = True
except:
rospy.logerr('No gripper found, gripper control disabled')
self._has_gripper = False
self._robot_enable = iif.RobotEnable(True)
self._params = iif.RobotParams()
self.blocking = False
# set up internal pybullet simulation for jacobian and mass matrix calcs.
self._clid = pb.connect(pb.DIRECT)
pb.resetSimulation(physicsClientId=self._clid)
# TODO: make this not hard coded
sawyer_urdf_path = self.config['sawyer']['arm']['path']
self._pb_sawyer = pb.loadURDF(
fileName=sawyer_urdf_path,
basePosition=self.config['sawyer']['arm']['pose'],
baseOrientation=pb.getQuaternionFromEuler(
self.config['sawyer']['arm']['orn']),
globalScaling=1.0,
useFixedBase=self.config['sawyer']['arm']['is_static'],
flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT
| pb.URDF_USE_INERTIA_FROM_FILE,
physicsClientId=self._clid)
# For pybullet dof
self._motor_joint_positions = self.get_motor_joint_positions()
try:
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
self._ik_service = True
except:
rospy.logerr('IKService from Intera timed out')
self._ik_service = False
self._joint_names = self.config['sawyer']['limb_joint_names']
self.free_joint_dict = self.get_free_joint_dict()
self.joint_dict = self.get_joint_dict()
self._link_id_dict = self.get_link_dict()
rospy.loginfo('Sawyer initialization finished after {} seconds'.format(
time.time() - start))
# Set desired pose to initial
self.neutral_joint_position = self.config['sawyer'][
'neutral_joint_angles'] #[0, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161]
self.prev_cmd = np.asarray(self.neutral_joint_position)
self.current_cmd = self.prev_cmd
self.reset_to_neutral()
self.default_control_type = self.config['controller']['selected_type']
self.default_params = self.config['controller']['Real'][
self.default_control_type]
self.redisClient.set(CONTROLLER_CONTROL_TYPE_KEY,
self.default_control_type)
self.redisClient.set(CONTROLLER_CONTROL_PARAMS_KEY,
json.dumps(self.default_params))
self.pb_ee_pos_log = []
self.pb_ee_ori_log = []
self.pb_ee_v_log = []
self.pb_ee_w_log = []
# Set initial redis keys
self._linear_jacobian = None
self._angular_jacobian = None
self._jacobian = None
self._calc_mass_matrix()
self._calc_jacobian()
self.update_redis()
self.update_model()
self.controlType = self.get_control_type()
self.control_dict = self.get_controller_params()
self.controller = self.make_controller_from_redis(
self.controlType, self.control_dict)
self.redisClient.set(ROBOT_CMD_TSTAMP_KEY, time.time())
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, 0.1)
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time())
self.last_cmd_tstamp = self.get_cmd_tstamp()
self.last_gripper_cmd_tstamp = self.get_gripper_cmd_tstamp()
self.prev_gripper_state = self.des_gripper_state
rospy.logdebug('Control Interface initialized')
# TIMING TEST ONLY
self.timelog_start = open('cmd_tstamp.txt', 'w')
self.timelog_end = open('cmd_set_time.txt', 'w')
self.controller_times = []
self.loop_times = []
self.cmd_end_time = []
print("Boundary position is: {}".format(ee_position))
response = input("Press [y] to confirm or [n] to resample.\n")
if response in ['n', 'N']:
pass
elif response in ['y', 'Y']:
print("position saved.")
boundaries[boundary_names[index]] = ee_position
collecting = False
else:
print("Invalid selection. Choose from [y] or [n]")
print("Boundary data collected: ")
print(boundaries)
yaml_fname = args.output_fname
with open(yaml_fname, 'w') as f:
safenet_yaml = {'safenet' : {}}
#safenet_yaml = YamlConfig('safenet.yaml')
safenet_yaml['safenet']['use_safenet'] = True
safenet_yaml['safenet']['upper'] = [boundaries['MAX X'][0], boundaries['MAX Y'][1], boundaries['MAX Z'][2]]
safenet_yaml['safenet']['lower'] = [boundaries['MIN X'][0], boundaries['MIN Y'][1], boundaries['MIN Z'][2]]
yaml.dump(safenet_yaml, f, allow_unicode=True)
pb_env = SafenetBoundaryEnv(world_type='Bullet', safenet_config=YamlConfig(yaml_fname))
input("Boundaries visualized. Press Enter to exit.")
assert len(env.robot_interface.q) == 7
assert len(env.robot_interface.dq) == 7
assert len(env.robot_interface.tau) == 7
assert len(env.robot_interface.ee_position) == 3
assert len(env.robot_interface.ee_orientation) == 4
assert len(env.robot_interface.ee_pose) == 7
assert len(env.robot_interface.ee_pose_euler) == 6
assert len(env.robot_interface.ee_v) == 3
assert len(env.robot_interface.ee_w) == 3
assert len(env.robot_interface.ee_twist) == 6
assert np.shape(env.robot_interface.rotation_matrix) == (9, )
assert np.shape(env.robot_interface.jacobian) == (6, 7)
assert np.shape(env.robot_interface.linear_jacobian) == (3, 7)
assert np.shape(env.robot_interface.angular_jacobian) == (3, 7)
assert np.shape(env.robot_interface.mass_matrix) == (7, 7)
assert len(env.robot_interface.N_q) == 7
env = Env(config='dev/test/test_bullet_cfg.yaml',
use_visualizer=False,
name='BulletSawyerTestEnv')
get_robot_states(env)
# Change config to panda.
test_config = YamlConfig('dev/test/test_bullet_cfg.yaml')
test_config['world']['robot'] = 'panda'
env = Env(config=test_config, use_visualizer=False, name='BulletPandaTestEnv')
get_robot_states(env)
ROBOT_STATE_EE_V_KEY: str(list(_limb.endpoint_velocity()['linear'])),
ROBOT_STATE_Q_KEY: str( _limb.joint_ordered_angles()),
ROBOT_STATE_DQ_KEY: json.dumps(_limb.joint_velocities()),
ROBOT_STATE_TAU_KEY: json.dumps(_limb.joint_efforts()),
ROBOT_MODEL_JACOBIAN_KEY: str(np.zeros((6,7))),
ROBOT_MODEL_L_JACOBIAN_KEY: str(np.zeros((3,6))),
ROBOT_MODEL_A_JACOBIAN_KEY: str(np.zeros((3,6))),
ROBOT_MODEL_MASS_MATRIX_KEY: str(np.zeros((7,7)))
}
redisClient.mset(robot_state)
print("Initializing ros redis interface.")
rospy.init_node("ros_redis_interface")
_limb = iif.Limb(limb="right", synchronous_pub=False)
config = YamlConfig('cfg/sawyer_ctrl_config.yaml')
# redis_kwargs = config['redis']
# if 'localhost' not in redis_kwargs['host']:
# redis_kwargs['host'] = socket.gethostbyname(config['redis']['host'])
# redisClient = redis.Redis(**redis_kwargs)
# redisClient.flushall()
redisClient = RobotRedis(**config['redis'])
joint_state_topic = 'robot/joint_states'
_joint_state_sub = rospy.Subscriber(
joint_state_topic,
JointState,
on_joint_states,
queue_size=1,
pass
elif response in ['y', 'Y']:
print("position saved.")
boundaries[boundary_names[index]] = ee_position
collecting = False
else:
print("Invalid selection. Choose from [y] or [n]")
print("Boundary data collected: ")
print(boundaries)
yaml_fname = args.output_fname
with open(yaml_fname, 'w') as f:
safenet_yaml = {'safenet': {}}
#safenet_yaml = YamlConfig('safenet.yaml')
safenet_yaml['safenet']['use_safenet'] = True
safenet_yaml['safenet']['upper'] = [
boundaries['MAX X'][0], boundaries['MAX Y'][1],
boundaries['MAX Z'][2]
]
safenet_yaml['safenet']['lower'] = [
boundaries['MIN X'][0], boundaries['MIN Y'][1],
boundaries['MIN Z'][2]
]
yaml.dump(safenet_yaml, f, allow_unicode=True)
pb_env = SafenetBoundaryEnv(world_type='Bullet',
safenet_config=YamlConfig(yaml_fname))
input("Boundaries visualized. Press Enter to exit.")