Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Peg Insertion Environment #37

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
+238 −0
Open

Peg Insertion Environment #37

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3f7ab8d
added peg insertion
ashwinreddy Jun 28, 2017
5bcddc8
fix terminal condition
ashwinreddy Jun 28, 2017
66311f1
Merge remote-tracking branch 'upstream/master'
ashwinreddy Aug 10, 2017
b499dc2
Update gym_peg_insertion.py
Aug 10, 2017
dd89901
fix reward
ashwinreddy Aug 13, 2017
3fc78a1
fixed merge conflict
ashwinreddy Aug 13, 2017
bb4813a
updated model file
ashwinreddy Aug 13, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 29 additions & 0 deletions agent_zoo/demo_peg_insertion.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
import os, sys, subprocess
import numpy as np
import gym
import roboschool

env = gym.make('RoboschoolPegInsertion-v0')
while 1:
frame = 0
score = 0
restart_delay = 0
obs = env.reset()

while 1:
a = env.action_space.sample()
# a = np.array([0]*7)
obs, r, done, _ = env.step(a)
score += r
frame += 1
still_open = env.render("rgb_array")
# import cv2
# cv2.imwrite('image.jpg', still_open)

if not done: continue
if restart_delay==0:
print("score=%0.2f in %i frames" % (score, frame))
restart_delay = 60*2 # 2 sec at 60 fps
else:
restart_delay -= 1
if restart_delay==0: break
6 changes: 6 additions & 0 deletions roboschool/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,11 @@
entry_point='roboschool:RoboschoolPong',
max_episode_steps=1000
)
register(
id='RoboschoolPegInsertion-v1',
entry_point='roboschool:RoboschoolPegInsertion',
max_episode_steps=1000
)

from roboschool.gym_pendulums import RoboschoolInvertedPendulum
from roboschool.gym_pendulums import RoboschoolInvertedPendulumSwingup
Expand All @@ -102,3 +107,4 @@
from roboschool.gym_humanoid_flagrun import RoboschoolHumanoidFlagrunHarder
from roboschool.gym_atlas import RoboschoolAtlasForwardWalk
from roboschool.gym_pong import RoboschoolPong
from roboschool.gym_peg_insertion import RoboschoolPegInsertion
66 changes: 66 additions & 0 deletions roboschool/gym_peg_insertion.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
import gym, roboschool
import numpy as np
from roboschool.gym_mujoco_xml_env import RoboschoolMujocoXmlEnv
from roboschool.scene_abstract import SingleRobotEmptyScene
import os, sys

class RoboschoolPegInsertion(RoboschoolMujocoXmlEnv):
def __init__(self):
RoboschoolMujocoXmlEnv.__init__(self, 'peg_insertion_arm.xml', 'arm', action_dim=7, obs_dim=110)

def create_single_player_scene(self):
return SingleRobotEmptyScene(gravity=9.8, timestep=0.01, frame_skip=1)

def robot_specific_reset(self):
for name, joint in self.jdict.items():
joint.reset_current_position(np.random.uniform(low=-.1, high=.1),0)
joint.set_motor_torque(0)

def calc_state(self):
part_coords = []
for name, part in self.parts.items():
# NOTE: received errors trying to do this the straightforward way
def add_to_parts_list(x):
part_coords.append([x[0], x[1], x[2]])
add_to_parts_list(part.pose().xyz())
joint_pos_s = []
for name, joint in self.jdict.items():
joint_pos_s.append(joint.current_position())
result = np.append(np.array(part_coords), np.array(joint_pos_s))
return result

def _step(self, action):
self.apply_action(action)
self.scene.global_step()

state = self.calc_state()

consts = dict(w_u=1e-6, w_p=1, alpha=0)
pose = self.parts['ball'].pose()

p_x_t = pose.xyz()
p_star = np.array([0, 0.3, -0.47])
diff = p_x_t-p_star

loss = .5 * consts['w_u'] * np.linalg.norm(action)**2
def l12(z):
return .5*np.linalg.norm(z)**2+np.sqrt(consts['alpha'] + z**2)

loss += consts['w_p']*l12(diff)
self.rewards = [-np.sum(loss)]
done = True if np.linalg.norm(diff) == 0 else False

self.HUD(state, action, done)
return state, self.rewards[0], done, {}

def apply_action(self, a):
assert(np.isfinite(a).all())
idx = 0
for name, joint in self.jdict.items():
action = 100*float(np.clip(a[idx], -1, +1))
joint.set_motor_torque(action)
idx += 1

def camera_adjust(self):
# self.camera.move_and_look_at(0,1.2,1.2, 0,0,0.5)
self.camera.move_and_look_at(0, 0, -0.188, 0.0, 0.3, -0.55)
137 changes: 137 additions & 0 deletions roboschool/mujoco_assets/peg_insertion_arm.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
<mujoco model="arm3d">

<compiler inertiafromgeom="true" angle="radian" coordinate="local" />
<option timestep="0.01" gravity="0 0 0" iterations="20" integrator="RK4" />
<default>
<joint armature="0.04" damping="1" limited="true" />
<geom friction=".5 .1 .1" margin="0.002" condim="1" contype="0" conaffinity="0" />
</default>

<worldbody>
<body name="arm">

<body name="r_shoulder_pan_link" pos="0 -0.188 0">
<geom name="e1" type="sphere" rgba="0.6 0.6 0.6 1" pos="-0.06 0.05 0.2" size="0.05" />
<geom name="e2" type="sphere" rgba="0.6 0.6 0.6 1" pos=" 0.06 0.05 0.2" size="0.05" />
<geom name="e1p" type="sphere" rgba="0.1 0.1 0.1 1" pos="-0.06 0.09 0.2" size="0.03" />
<geom name="e2p" type="sphere" rgba="0.1 0.1 0.1 1" pos=" 0.06 0.09 0.2" size="0.03" />
<geom name="sp" type="capsule" fromto="0 0 -0.4 0 0 0.2" size="0.1" />
<joint name="r_shoulder_pan_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="-2.2854 1.714602" damping="10.0" />

<body name="r_shoulder_lift_link" pos="0.1 0 0">
<geom name="sl" type="capsule" fromto="0 -0.1 0 0 0.1 0" size="0.1" />
<joint name="r_shoulder_lift_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-0.5236 1.3963" damping="10.0" />

<body name="r_upper_arm_roll_link" pos="0 0 0">
<geom name="uar" type="capsule" fromto="-0.1 0 0 0.1 0 0" size="0.02" />
<joint name="r_upper_arm_roll_joint" type="hinge" pos="0 0 0" axis="1 0 0" range="-3.9 0.8" damping="0.1" />

<body name="r_upper_arm_link" pos="0 0 0">
<geom name="ua" type="capsule" fromto="0 0 0 0.4 0 0" size="0.06" />

<body name="r_elbow_flex_link" pos="0.4 0 0">
<geom name="ef" type="capsule" fromto="0 -0.02 0 0.0 0.02 0" size="0.06" />
<joint name="r_elbow_flex_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.3213 0" damping="1.0" />

<body name="r_forearm_roll_link" pos="0 0 0">
<geom name="fr" type="capsule" fromto="-0.1 0 0 0.1 0 0" size="0.02" />
<joint name="r_forearm_roll_joint" type="hinge" limited="false" pos="0 0 0" axis="1 0 0" damping=".1" />

<body name="r_forearm_link" pos="0 0 0">
<geom name="fa" type="capsule" fromto="0 0 0 0.321 0 0" size="0.05" />

<body name="r_wrist_flex_link" pos="0.321 0 0">
<geom name="wf" type="capsule" fromto="0 -0.02 0 0 0.02 0" size="0.01" />
<joint name="r_wrist_flex_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.094 0" damping=".1" />

<body name="r_wrist_roll_link" pos="0 0 0">
<geom name="wr" type="capsule" fromto="-0.02 0 0 0.02 0 0" size="0.01" />
<joint name="r_wrist_roll_joint" type="hinge" pos="0 0 0" limited="false" axis="1 0 0" damping="0.1" />

<body name="r_gripper_palm_link" pos="0 0 0">
<geom name="pl" type="capsule" fromto="0.05 0 -0.02 0.05 0 0.02" size="0.05" />

<body name="r_gripper_tool_frame" pos="0.18 0 0">
<site name="leg_bottom" pos="0 0 -0.15" size="0.01" />
<site name="leg_top" pos="0 0 0.15" size="0.01" />

<body name="ball" pos="0 0 0">
<geom name="ball_geom" rgba="0.8 0.6 0.6 1" type="cylinder" fromto="0 0 -0.15 0 0 0.15" size="0.028" density="2000" contype="2" conaffinity="1" />
</body>
</body>

<body name="r_gripper_l_finger_link" pos="0.07691 0.03 0">
<geom name="gf3" type="capsule" fromto="0 0 0 0.09137 0.00495 0" size="0.01" />

<body name="r_gripper_l_finger_tip_link" pos="0.09137 0.00495 0">
<geom name="gf4" type="capsule" fromto="0 0 0 0.09137 0.0 0" size="0.01" />
</body>
</body>

<body name="r_gripper_r_finger_link" pos="0.07691 -0.03 0">
<geom name="gf1" type="capsule" fromto="0 0 0 0.09137 -0.00495 0" size="0.01" />

<body name="r_gripper_r_finger_tip_link" pos="0.09137 -0.00495 0">
<geom name="gf2" type="capsule" fromto="0 0 0 0.09137 0.0 0" size="0.01" />
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>

<body name="g1" pos="0.034 0.3 -0.47" axisangle="0 1 0 0.05">
<geom name="g1" rgba="0.2 0.2 0.2 1" type="box" size="0.003 0.01 0.05" contype="1" conaffinity="1" />
</body>

<body name="g2" pos="-0.034 0.3 -0.47" axisangle="0 1 0 -0.05">
<geom name="g2" rgba="0.2 0.2 0.2 1" type="box" size="0.003 0.01 0.05" contype="1" conaffinity="1" />
</body>

<body name="g3" pos="0.0 0.334 -0.47" axisangle="1 0 0 -0.05">
<geom name="g3" rgba="0.2 0.2 0.2 1" type="box" size="0.01 0.003 0.05" contype="1" conaffinity="1" />
</body>

<body name="g4" pos="0.0 0.266 -0.47" axisangle="1 0 0 0.05">
<geom name="g4" rgba="0.2 0.2 0.2 1" type="box" size="0.01 0.003 0.05" contype="1" conaffinity="1" />
</body>

<body name="fl" pos="0.0 0.3 -0.55">
<geom name="fl" rgba="0.2 0.2 0.2 1" type="box" size="0.2 0.2 0.05" contype="1" conaffinity="1" />
</body>

<body name="w1" pos="0.216 0.3 -0.45">
<geom name="w1" rgba="0.2 0.2 0.2 1" type="box" size="0.183 0.3 0.05" contype="1" conaffinity="1" />
</body>

<body name="w2" pos="-0.216 0.3 -0.45">
<geom name="w2" rgba="0.2 0.2 0.2 1" type="box" size="0.183 0.3 0.05" contype="1" conaffinity="1" />
</body>

<body name="w3" pos="0.0 0.516 -0.45">
<geom name="w3" rgba="0.2 0.2 0.2 1" type="box" size="0.032 0.183 0.05" contype="1" conaffinity="1" />
</body>

<body name="w4" pos="0.0 0.084 -0.45">
<geom name="w4" rgba="0.2 0.2 0.2 1" type="box" size="0.032 0.183 0.05" contype="1" conaffinity="1" />
</body>
</body>
</worldbody>

<actuator>
<motor joint="r_shoulder_pan_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
<motor joint="r_shoulder_lift_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
<motor joint="r_upper_arm_roll_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
<motor joint="r_elbow_flex_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
<motor joint="r_forearm_roll_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
<motor joint="r_wrist_flex_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
<motor joint="r_wrist_roll_joint" ctrlrange="-100.0 100.0" ctrllimited="true" />
</actuator>

</mujoco>