diff --git a/.dockerignore b/.dockerignore
old mode 100644
new mode 100755
diff --git a/.flake8 b/.flake8
old mode 100644
new mode 100755
diff --git a/.gitignore b/.gitignore
old mode 100644
new mode 100755
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
old mode 100644
new mode 100755
diff --git a/.travis.yml b/.travis.yml
old mode 100644
new mode 100755
diff --git a/CMakeLists.txt b/CMakeLists.txt
old mode 100644
new mode 100755
diff --git a/Dockerfile b/Dockerfile
old mode 100644
new mode 100755
diff --git a/Makefile b/Makefile
old mode 100644
new mode 100755
diff --git a/README.md b/README.md
old mode 100644
new mode 100755
diff --git a/__init__.py b/__init__.py
old mode 100644
new mode 100755
diff --git a/docker_train.sh b/docker_train.sh
old mode 100644
new mode 100755
diff --git a/launch/open_manipulator_env.launch b/launch/open_manipulator_env.launch
index 038b450..26c0ef3 100755
--- a/launch/open_manipulator_env.launch
+++ b/launch/open_manipulator_env.launch
@@ -7,10 +7,12 @@
-
-
-
-
+
+
+
+
+
+
@@ -23,28 +25,21 @@
-
-
-
-
- ["$(arg robot_name)/joint_states"]
-
-
-
+
+
-
-
-
-
-
+
+
+
+
-
+
-
+
diff --git a/msg/GoalObs.msg b/msg/GoalObs.msg
old mode 100644
new mode 100755
diff --git a/package.xml b/package.xml
old mode 100644
new mode 100755
diff --git a/scripts/.isort.cfg b/scripts/.isort.cfg
old mode 100644
new mode 100755
diff --git a/scripts/__init__.py b/scripts/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/__init__.py b/scripts/algorithms/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/__init__.py b/scripts/algorithms/common/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/abstract/__init__.py b/scripts/algorithms/common/abstract/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/__init__.py b/scripts/algorithms/common/buffer/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/priortized_replay_buffer.py b/scripts/algorithms/common/buffer/priortized_replay_buffer.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/replay_buffer.py b/scripts/algorithms/common/buffer/replay_buffer.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/buffer/segment_tree.py b/scripts/algorithms/common/buffer/segment_tree.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/helper_functions.py b/scripts/algorithms/common/helper_functions.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/networks/__init__.py b/scripts/algorithms/common/networks/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/networks/lstm.py b/scripts/algorithms/common/networks/lstm.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/networks/mlp.py b/scripts/algorithms/common/networks/mlp.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/common/noise.py b/scripts/algorithms/common/noise.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/fd/__init__.py b/scripts/algorithms/fd/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/fd/sac_agent.py b/scripts/algorithms/fd/sac_agent.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/fd/td3_agent.py b/scripts/algorithms/fd/td3_agent.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/sac/__init__.py b/scripts/algorithms/sac/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/sac/agent.py b/scripts/algorithms/sac/agent.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/td3/__init__.py b/scripts/algorithms/td3/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/algorithms/td3/agent.py b/scripts/algorithms/td3/agent.py
old mode 100644
new mode 100755
diff --git a/scripts/config/__init__.py b/scripts/config/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/__init__.py b/scripts/config/agent/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/__init__.py b/scripts/config/agent/lunarlander_continuous_v2/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/sac.py b/scripts/config/agent/lunarlander_continuous_v2/sac.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/sacfd.py b/scripts/config/agent/lunarlander_continuous_v2/sacfd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/td3.py b/scripts/config/agent/lunarlander_continuous_v2/td3.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/lunarlander_continuous_v2/td3fd.py b/scripts/config/agent/lunarlander_continuous_v2/td3fd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/__init__.py b/scripts/config/agent/reacher-v1/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/sac.py b/scripts/config/agent/reacher-v1/sac.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/sacfd.py b/scripts/config/agent/reacher-v1/sacfd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/td3.py b/scripts/config/agent/reacher-v1/td3.py
old mode 100644
new mode 100755
diff --git a/scripts/config/agent/reacher-v1/td3fd.py b/scripts/config/agent/reacher-v1/td3fd.py
old mode 100644
new mode 100755
diff --git a/scripts/config/environment/__init__.py b/scripts/config/environment/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/data/lunarlander_continuous_demo.pkl b/scripts/data/lunarlander_continuous_demo.pkl
old mode 100644
new mode 100755
diff --git a/scripts/data/reacher_demo.pkl b/scripts/data/reacher_demo.pkl
old mode 100644
new mode 100755
diff --git a/scripts/envs/__init__.py b/scripts/envs/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/envs/open_manipulator/__init__.py b/scripts/envs/open_manipulator/__init__.py
old mode 100644
new mode 100755
diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py
index cd3978f..34909f7 100755
--- a/scripts/envs/open_manipulator/ros_interface.py
+++ b/scripts/envs/open_manipulator/ros_interface.py
@@ -11,7 +11,9 @@ import rospkg # noqa
import rospy # noqa
import tf # noqa
import tf.transformations as tr # noqa
-from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel
+from urdf_parser_py.urdf import URDF # noqa
+from pykdl_utils.kdl_kinematics import KDLKinematics
+from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel # noqa
from geometry_msgs.msg import Pose
from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState
from sensor_msgs.msg import JointState
@@ -37,6 +39,7 @@ class OpenManipulatorRosBaseInterface(object):
self.termination_count = 0
self.success_count = 0
+ self.init_fk_solver()
self.init_tf_transformer()
self.init_publish_node()
self.init_subscribe_node()
@@ -116,6 +119,10 @@ class OpenManipulatorRosBaseInterface(object):
self.pub_joint3_position.publish(np.random.uniform(0.0, 0.0))
self.pub_joint4_position.publish(np.random.uniform(0.0, 0.0))
+ def init_fk_solver(self):
+ self.robot = URDF.from_parameter_server()
+ self.solve_fk = KDLKinematics(self.robot, "link1", "end_effector_link")
+
def joint_state_callback(self, msg):
"""Callback function of joint states subscriber.
@@ -129,19 +136,16 @@ class OpenManipulatorRosBaseInterface(object):
self.joint_efforts = joints_states.effort
# penalize jerky motion in reward for shaped reward setting.
self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
- try:
- (
- self._gripper_position,
- self._gripper_orientation,
- ) = self.tf_listenser.lookupTransform(
- "/world", "/end_effector_link", rospy.Time(0)
- )
- except (
- tf.LookupException,
- tf.ConnectivityException,
- tf.ExtrapolationException,
- ):
- pass
+ _fk_mat = np.array(self.solve_fk.forward(self.joint_positions[:4]))
+ self._gripper_position = _fk_mat[0:3, 3]
+ self._gripper_orientation[3] = (1 + _fk_mat[0, 0] +
+ _fk_mat[1, 1] + _fk_mat[2, 2])**0.5
+ self._gripper_orientation[0] = (
+ _fk_mat[2, 1] - _fk_mat[1, 2]) / (4 * self._gripper_orientation[3])
+ self._gripper_orientation[1] = (
+ _fk_mat[0, 2] - _fk_mat[2, 0]) / (4 * self._gripper_orientation[3])
+ self._gripper_orientation[2] = (
+ _fk_mat[1, 0] - _fk_mat[0, 1]) / (4 * self._gripper_orientation[3])
def kinematics_pose_callback(self, msg):
"""Callback function of gripper kinematic pose subscriber.
@@ -258,7 +262,7 @@ class OpenManipulatorRosBaseInterface(object):
)
else:
raise ValueError("Control mode %s is not known!" % control_mode)
- print (lower_bounds, upper_bounds, self.cfg["ACTION_DIM"])
+ print(lower_bounds, upper_bounds, self.cfg["ACTION_DIM"])
return gym.spaces.Box(low=lower_bounds, high=upper_bounds, dtype=np.float32)
def get_observation_space(self):
diff --git a/scripts/requirements-dev.txt b/scripts/requirements-dev.txt
old mode 100644
new mode 100755
diff --git a/scripts/requirements.txt b/scripts/requirements.txt
old mode 100644
new mode 100755
diff --git a/scripts/run_lunarlander_continuous.py b/scripts/run_lunarlander_continuous.py
old mode 100644
new mode 100755
diff --git a/scripts/run_reacher_v1.py b/scripts/run_reacher_v1.py
old mode 100644
new mode 100755
diff --git a/scripts/test_lstm.py b/scripts/test_lstm.py
old mode 100644
new mode 100755
diff --git a/urdf/om.urdf b/urdf/om.urdf
new file mode 100755
index 0000000..6367e64
--- /dev/null
+++ b/urdf/om.urdf
@@ -0,0 +1,308 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+