In [21]:

    

import rospy
import json
from os.path import join
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from human_moveit_config.human_model import HumanModel
from math import sin
from math import cos
import numpy as np
import tf
from baxter_commander.commander import ArmCommander
from baxter_commander.persistence import dicttotraj
from reba_optim.reba_assess import RebaAssess


    

    




Using Theano backend.

In [2]:

    

def quat_from_axis_angle(vect, angle):
    factor = sin(angle / 2.0)
    quat = factor * np.array(vect)
    quat = np.append(quat, [cos(angle / 2.0)])
    quat = quat / np.linalg.norm(quat)
    return quat.tolist()


    

In [3]:

    

rospy.init_node('test_mlr_simu')


    

In [4]:

    

pub = tf.TransformBroadcaster()


    

In [5]:

    

model = HumanModel(control=True)


    

In [22]:

    

reba = RebaAssess()


    

In [6]:

    

#right = ArmCommander("right")


    

In [7]:

    

#left = ArmCommander("left")


    

In [8]:

    

dirname = join('/home', 'buschbapti', 'Documents', 'mlr', 'share', 'projects', '17-LGP-comfort', 'data')


    

In [9]:

    

simuname = 'test2'


    

In [29]:

    

with open(join(dirname, simuname, 'human_traj.json')) as datafile:
    human_traj = json.load(datafile)


    

In [30]:

    

with open(join(dirname, simuname, 'right_robot_traj.json')) as datafile:
    r_robot_traj = json.load(datafile)


    

In [31]:

    

with open(join(dirname, simuname, 'left_robot_traj.json')) as datafile:
    l_robot_traj = json.load(datafile)


    

In [32]:

    

rate = rospy.Rate(5)


    

In [33]:

    

human_pose = [[1.4, 0.0, 0.0], quat_from_axis_angle([0, 0, 1], np.pi)]


    

In [34]:

    

#right_traj = dicttotraj(r_robot_traj)
#right.display(right_traj)


    

In [35]:

    

#left_traj = dicttotraj(l_robot_traj)
#left.display(left_traj)


    

In [36]:

    

traj_modified = False


    

In [37]:

    

human_js = JointState()
human_js.name = human_traj['joint_names']
for i in range(len(human_traj['points'])):
    point = human_traj['points'][i]
    human_js.position = point['positions']
    model.send_state(human_js)
    pub.sendTransform(human_pose[0], human_pose[1], rospy.Time.now(), 'base', 'human/base')
    if 'posture_score' not in point.keys():
        score = reba.assess_posture(human_js, 'neural_network')
        point['posture_score'] = score
        traj_modified = True
    rate.sleep()


    

In [39]:

    

if traj_modified:
    with open(join(dirname, simuname, 'human_traj.json'), 'w') as outfile:
        json.dump(human_traj, outfile, indent=4)


    

    




[WARN] [WallTime: 1494340077.137421] Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.
[WARN] [WallTime: 1494340087.742553] Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.

In [ ]: