In [1]:

    

import rospy
from human_moveit_config.human_model import HumanModel
from sensor_msgs.msg import JointState
from os.path import join
import json
from rospkg import RosPack
from baxter_commander.persistence import dicttostate
import re

import plotly.plotly as py
import plotly.graph_objs as go
import pandas as pd
import numpy as np

from tools.unity_bridge import UnityBridge 
from copy import deepcopy
from trajectories.joint_trajectories import trapezoidal_speed_trajectory 
from copy import deepcopy


    

In [2]:

    

rospy.init_node('testFeedback')


    

In [3]:

    

bridge = UnityBridge()


    

In [4]:

    

rospack = RosPack()
postures_file = join(rospack.get_path('reba_optim'), 'tmp', 'postures.json')
with open(postures_file) as datafile:
    postures_data = json.load(datafile)


    

In [5]:

    

pub = rospy.Publisher('reba_assess', JointState, queue_size=10)


    

In [6]:

    

human = HumanModel(control=True)


    

In [7]:

    

rate = rospy.Rate(10)


    

In [8]:

    

####### RESET MODEL ########
T_state = human.get_initial_state()
human.send_state(T_state)
bridge.send_state(T_state)


    

In [ ]:

    

####### CLASS STATE TEST #######
nb_state = 4
class_value = 1
states = postures_data[class_value - 1]

s = states[nb_state]
goal_state = dicttostate(s).joint_state
human.send_state(goal_state) 
bridge.send_state(goal_state)


    

In [ ]:

    

jt = trapezoidal_speed_trajectory(T_state, goal_state, 1, 0.2, 10)


    

In [ ]:

    

for p in jt.points:
    state = JointState()
    state.name = jt.joint_names
    state.position = p.positions
    human.send_state(state)
    bridge.send_state(state)
    rate.sleep()


    

In [ ]:

    

listFeedback = ['spine', 'neck', 'left_shoulder', 'right_shoulder',
                'left_elbow', 'right_elbow', 'left_wrist', 'right_wrist']
feedbackValue = {f: np.random.rand() for f in listFeedback}
bridge.activate_risk_frames(feedbackValue)


    

In [ ]:

    

prev_state = dicttostate(states[0]).joint_state
prev_state.position = [0] * len(prev_state.name)
for s in states:
    new_state = dicttostate(s).joint_state
    path = trapezoidal_speed_trajectory(new_state, prev_state, 1, 0.2, 10)
    for p in path.points:
        state = JointState()
        state.name = path.joint_names
        state.position = p.positions
        human.send_state(state)
        bridge.send_state(state)
        rate.sleep()
    prev_state = new_state


    

In [11]:

    

joints = ['left_shoulder_0', 'left_elbow_0', 'right_shoulder_2', 'right_elbow_0', 'neck_2', 'right_wrist_1']
values = [1.12, -1.12, 0.78, 1.75, -0.3, -1.12]


    

In [12]:

    

new_state = deepcopy(T_state)
for i in range(len(joints)):
    new_state.position[new_state.name.index(joints[i])] = values[i]
path = trapezoidal_speed_trajectory(new_state, T_state, 1, 0.2)
for p in path.points:
    state = JointState()
    state.name = path.joint_names
    state.position = p.positions
    human.send_state(state)
    bridge.send_state(state)
    rate.sleep()


    

In [13]:

    

new_state = deepcopy(T_state)
for i in range(len(joints)):
    new_state.position[new_state.name.index(joints[i])] = values[i]
human.send_state(new_state)
bridge.send_state(new_state)


    

In [ ]:

    

bridge.activate_model(0)


    

In [ ]: