Reba Score

.gitignore

@@ -112,3 +112,12 @@ venv.bak/
 
 # mypy
 .mypy_cache/
+
+#idea 
+.idea/ 
+
+# meshes 
+assistive_gym/envs/assets/human/meshes/
+
+# trained_models
+trained_models/

README.md

@@ -51,6 +51,7 @@ cd assistive-gym
 pip3 install -e .
 ```
 
+
 ## Getting Started
 We provide a [10 Minute Getting Started Guide](https://github.com/Healthcare-Robotics/assistive-gym/wiki/3.-Getting-Started) to help you get familiar with using Assistive Gym for assistive robotics research.
 
@@ -146,3 +147,70 @@ This allows robots to learn to provide assistance that is consist with a person'
 ![Human preferences](images/human_preferences.gif "Human preferences")
 
 Refer to [the paper](https://arxiv.org/abs/1910.04700) for details on features in Assistive Gym.
+
+
+# Note on installation on Unbuntu 22
+- Need to use pyenv to install python 3.7.10
+```bash
+pyenv install 3.7.10
+pyenv local 3.7.10 
+```
+- If there is error when executing project, please install the following lib
+```
+sudo apt-get install python-tk python3-tk tk-dev
+sudo apt-get install build-essential zlib1g-dev libffi-dev libssl-dev libbz2-dev libreadline-dev libsqlite3-dev liblzma-dev
+```
+- There is backward incompatible change in gym package. To fix it, we need to modify the source code in `/.pyenv/versions/3.7.10/lib/python3.7/site-packages/gym/wrappers/time_limit.py`
+Please refer to https://stackoverflow.com/questions/74060371/gym-super-mario-bros-7-3-0-valueerror-not-enough-values-to-unpack-expected
+
+- Missing installations
+```
+# downgrade protobuf 
+pip3 install protobuf==3.20.*
+
+
+pip3 install pytorch3
+
+pip3 install opencv-python
+
+pip3 install matplotlib
+
+pip3 install chumpy
+pip3 install open3d # for trimesh decimation
+```
+Added lib for geom 
+```
+numpy-stl
+vtk
+```
+
+# Train human comfort
+## Flag meaning
+- Supported train/ render mode: 
+  - train with gui: `--render-gui --train`
+  - train without gui: `--train`
+  - render train result: `--render`
+
+- Supported handover object: 
+    - single object: `--handover-object "cup"/ "cane"/ "pill"`
+    - train all objects: `--handover-obj 'all'`
+
+- Supported train mode:
+  - with robot ik and real handover object in loop: `--robot-ik`
+  - without robot ik and simulated collision object on end-effector (default): omit `--robot-ik` flag
+
+## Command
+```bash
+# pyenv eval "$(pyenv init -)" -> python 3.7.10
+cd assistive-gym/
+# train
+python3 -m assistive_gym.train --env "HumanComfort-v1" --smpl-file "examples/data/smpl_bp_ros_smpl_4.pkl" --save-dir "trained_models" --train --render-gui --robot-ik --handover-obj 'cane'
+
+# render
+python3 -m assistive_gym.train --env "HumanComfort-v1" --smpl-file "examples/data/smpl_bp_ros_smpl_4.pkl" --save-dir "trained_models" --render --handover-obj 'cane'
+```
+## Batch training 
+Modify train.sh script and run 
+```bash
+./train.sh
+```

assistive_gym/__init__.py

@@ -31,3 +31,23 @@
     max_episode_steps=200,
 )
 
+# Just added
+register(
+    id='HumanResting-v1',
+    entry_point='assistive_gym.envs:HumanRestingEnv',
+    max_episode_steps=200,
+)
+
+
+register(
+    id='HumanLying-v1',
+    entry_point='assistive_gym.envs:HumanLyingEnv',
+    max_episode_steps=200,
+)
+
+
+register(
+    id='HumanComfort-v1',
+    entry_point='assistive_gym.envs:HumanComfortEnv',
+    max_episode_steps=200,
+)

assistive_gym/env_viewer.py

@@ -15,21 +15,21 @@ def sample_action(env, coop):
 def viewer(env_name):
     coop = 'Human' in env_name
     env = make_env(env_name, coop=True) if coop else gym.make(env_name)
-
+    env.reset()
     while True:
         done = False
         env.render()
         observation = env.reset()
         action = sample_action(env, coop)
-        if coop:
-            print('Robot observation size:', np.shape(observation['robot']), 'Human observation size:', np.shape(observation['human']), 'Robot action size:', np.shape(action['robot']), 'Human action size:', np.shape(action['human']))
-        else:
-            print('Observation size:', np.shape(observation), 'Action size:', np.shape(action))
+        # if coop:
+        #     print('Robot observation size:', np.shape(observation['robot']), 'Human observation size:', np.shape(observation['human']), 'Robot action size:', np.shape(action['robot']), 'Human action size:', np.shape(action['human']))
+        # else:
+        #     print('Observation size:', np.shape(observation), 'Action size:', np.shape(action))
 
-        while not done:
+        while 1:
             observation, reward, done, info = env.step(sample_action(env, coop))
-            if coop:
-                done = done['__all__']
+            # if coop:
+            #     done = done['__all__']
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(description='Assistive Gym Environment Viewer')

assistive_gym/envs/__init__.py

@@ -8,3 +8,7 @@
 from assistive_gym.envs.arm_manipulation_envs import ArmManipulationPR2Env, ArmManipulationBaxterEnv, ArmManipulationSawyerEnv, ArmManipulationJacoEnv, ArmManipulationStretchEnv, ArmManipulationPandaEnv, ArmManipulationPR2HumanEnv, ArmManipulationBaxterHumanEnv, ArmManipulationSawyerHumanEnv, ArmManipulationJacoHumanEnv, ArmManipulationStretchHumanEnv, ArmManipulationPandaHumanEnv
 from assistive_gym.envs.human_testing import HumanTestingEnv
 from assistive_gym.envs.smplx_testing import SMPLXTestingEnv
+from assistive_gym.envs.human_resting import HumanRestingEnv
+from assistive_gym.envs.human_lying import HumanLyingEnv
+from assistive_gym.envs.human_comfort_env import HumanComfortEnv
+

assistive_gym/envs/agents/agent.py

@@ -1,6 +1,7 @@
 import numpy as np
 import pybullet as p
 
+
 class Agent:
     def __init__(self):
         self.base = -1
@@ -19,6 +20,7 @@ def init(self, body, id, np_random, indices=None):
         self.id = id
         self.np_random = np_random
         self.all_joint_indices = list(range(p.getNumJoints(body, physicsClientId=id)))
+        # print ("body: ", body,  " all_joint_indices: ", self.all_joint_indices)
         if indices != -1:
             self.update_joint_limits()
             self.enforce_joint_limits(indices)
@@ -37,6 +39,7 @@ def get_joint_angles(self, indices=None):
             indices = self.all_joint_indices
         elif not indices:
             return []
+        # print(self.body, " indices: ", indices)
         robot_joint_states = p.getJointStates(self.body, jointIndices=indices, physicsClientId=self.id)
         return np.array([x[0] for x in robot_joint_states])
 
@@ -116,7 +119,10 @@ def get_contact_points(self, agentB=None, linkA=None, linkB=None):
         return linkA, linkB, posA, posB, force
 
     def get_closest_points(self, agentB, distance=4.0, linkA=None, linkB=None):
-        args = dict(bodyA=self.body, bodyB=agentB.body, distance=distance, physicsClientId=self.id)
+        return self.get_closest_points_with_body_id(agentB.body, distance, linkA, linkB)
+
+    def get_closest_points_with_body_id(self, bodyB, distance=4.0, linkA=None, linkB=None):
+        args = dict(bodyA=self.body, bodyB=bodyB, distance=distance, physicsClientId=self.id)
         if linkA is not None:
             args['linkIndexA'] = linkA
         if linkB is not None:
@@ -153,6 +159,7 @@ def set_base_velocity(self, linear_velocity, angular_velocity):
 
     def set_joint_angles(self, indices, angles, use_limits=True, velocities=0):
         for i, (j, a) in enumerate(zip(indices, angles)):
+            # print ("j: ", self.lower_limits[j], self.upper_limits[j])
             p.resetJointState(self.body, jointIndex=j, targetValue=min(max(a, self.lower_limits[j]), self.upper_limits[j]) if use_limits else a, targetVelocity=velocities if type(velocities) in [int, float] else velocities[i], physicsClientId=self.id)
 
     def set_on_ground(self, base_height=None):
@@ -194,7 +201,8 @@ def set_joint_stiffness(self, joint, stiffness):
         # p.changeDynamics(self.body, joint, contactStiffness=stiffness, contactDamping=stiffness, physicsClientId=self.id)
 
     def set_gravity(self, ax=0.0, ay=0.0, az=-9.81):
-        p.setGravity(ax, ay, az, body=self.body, physicsClientId=self.id)
+        # p.setGravity(ax, ay, az, body=self.body, physicsClientId=self.id)
+        p.setGravity(ax, ay, az, physicsClientId=self.id)
 
     def enable_force_torque_sensor(self, joint):
         p.enableJointForceTorqueSensor(self.body, joint, enableSensor=True, physicsClientId=self.id)
@@ -282,3 +290,40 @@ def print_joint_info(self, show_fixed=True):
                 joint_names.append((j, info[1]))
         print(joint_names)
 
+    def add_collision_object_around_link(self, link_idx, radius=0.05):
+        """
+        Adds a collision object around a link for end effector clearance check
+        :param link_idx:
+        :param radius:
+        :return:
+        """
+        def create_sphere(radius=0, position_offset=[0, 0, 0], orientation=[0, 0, 0, 1]):
+            visual_shape = p.createVisualShape(p.GEOM_SPHERE, radius=radius,
+                                               visualFramePosition=position_offset,
+                                               rgbaColor=[1, 0, 0, 0.5],
+                                               visualFrameOrientation=orientation, physicsClientId=self.id)
+            collision_shape = p.createCollisionShape(p.GEOM_SPHERE, radius=radius,
+                                                     collisionFramePosition=position_offset,
+                                                     collisionFrameOrientation=orientation, physicsClientId=self.id)
+            return visual_shape, collision_shape
+
+        link_pos, link_orient = p.getLinkState(self.body, link_idx, physicsClientId=self.id)[:2]
+
+        shape_visual, shape_collision = create_sphere(radius=radius, position_offset=[0, 0, 0.0],
+                                                       orientation=p.getQuaternionFromEuler([0, 0, 0]))
+        collision_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=shape_collision,
+                                 baseVisualShapeIndex=shape_visual, basePosition=link_pos,useMaximalCoordinates=False,
+                                 baseOrientation=link_orient, physicsClientId=self.id)
+
+        # create fake link constraint
+        pos_offset = [0, 0, 0]
+        orient_offset = [0, 0, 0, 1]
+        constraint = p.createConstraint(self.body, link_idx,
+                                        collision_body, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset,
+                                        childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset,
+                                        childFrameOrientation=[0, 0, 0, 1], physicsClientId=self.id)
+        p.changeConstraint(constraint, maxForce=500000, physicsClientId=self.id)
+
+        # ignore the collision between the collision body and the robot
+        p.setCollisionFilterPair(self.body, collision_body, link_idx, -1, 0, physicsClientId=self.id)
+        return collision_body

assistive_gym/envs/agents/human.py

@@ -126,7 +126,7 @@ def setup_joints(self, joints_positions, use_static_joints=True, reactive_force=
             forces = [reactive_force * self.strength] * len(self.target_joint_angles)
             self.control(self.controllable_joint_indices, self.target_joint_angles, reactive_gain, forces)
 
-    def get_body_params():
+    def get_body_params(self):
         body_shape = np.zeros(10)
         joint_ranges = np.zeros(21, 2).flatten()
         return np.concatenate([body_shape, joint_ranges])

assistive_gym/envs/agents/human_mesh.py

@@ -137,7 +137,7 @@ def create_smplx_body(self, directory, id, np_random, gender='female', height=No
         # out_mesh.apply_transform(scale)
         # rot = trimesh.transformations.rotation_matrix(np.deg2rad(90), [1, 0, 0])
         # out_mesh.apply_transform(rot)
-
+        
         return out_mesh, vertices, joints
 
     def init(self, directory, id, np_random, gender='female', height=None, body_shape=None, joint_angles=[], position=[0, 0, 0], orientation=[0, 0, 0], skin_color='random', specular_color=[0.1, 0.1, 0.1], body_pose=None, out_mesh=None, vertices=None, joints=None):