def run(self, dummy=False):
rospy.loginfo("Torso is connecting to the robot...")
try:
self.torso = PoppyTorso(use_http=True, simulator='poppy-simu' if dummy else None)
except IOError as e:
rospy.logerr("Torso failed to init: {}".format(e))
return None
else:
self.primitive_head = UpperBodyIdleMotion(self.torso, 15)
self.primitive_right = HeadIdleMotion(self.torso, 15)
self.go_to([90, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0], 4)
self.primitive_head.start()
self.primitive_right.start()
try:
self.set_torque_limits()
self.torso.compliant = False
self.go_to_rest(True)
self.srv_reset = rospy.Service('/nips2016/torso/reset', Reset, self._cb_reset)
self.srv_execute = rospy.Service('/nips2016/torso/execute', ExecuteTorsoTrajectory, self._cb_execute)
self.srv_set_compliant = rospy.Service('/nips2016/torso/set_compliant', SetTorsoCompliant, self._cb_set_compliant)
rospy.loginfo("Torso is ready to execute trajectories at {} Hz ".format(self.execute_rate_hz))
while not rospy.is_shutdown():
self.publish_eef(self.torso.l_arm_chain.end_effector, self.eef_pub_l)
self.publish_eef(self.torso.r_arm_chain.end_effector, self.eef_pub_r)
self.publish_js()
self.publish_rate.sleep()
finally:
self.primitive_right.stop()
self.primitive_head.stop()
self.torso.close()
import sys
import numpy as np
import time
import os
x = sys.argv[1]
y = sys.argv[2]
z = sys.argv[3]
nb_objet = sys.argv[4]
from poppy.creatures import PoppyTorso
from cinematique import Bras, Buste
from couple import *
#instanciation du robot avec le nom marlien (Mar pour Martin et Lien pour Julien)
marlien = PoppyTorso() #Commande uniquement pour le vrai robot
marlien.compliant = False
#mise à 0 des moteurs
for m in marlien.motors:
m.goto_position(0, 2)
time.sleep(2)
coord_objet = np.mat([[float(x)], [float(y)], [float(z)]])
nb_objet = int(nb_objet)
marlien.l_shoulder_y.goto_position(-180, 3)
marlien.l_elbow_y.goto_position(90, 3)
marlien.r_shoulder_y.goto_position(-180, 3)
marlien.r_elbow_y.goto_position(90, 3, wait=True)
buste = Buste(coord_objet)
import time
import numpy as np
from poppy.creatures import PoppyTorso
poppy = PoppyTorso(simulator='vrep')
delay_time = 1
target_delta = np.array([-0.15, 0, 0])
# Initialize the robot
for m in poppy.motors:
m.goto_position(0, 2)
# Left arm is compliant, right arm is active
for m in poppy.l_arm:
m.compliant = False
for m in poppy.r_arm:
m.compliant = False
# The torso itself must not be compliant
for m in poppy.torso:
m.compliant = False
def follow_hand(poppy, delta):
"""Tell the right hand to follow the left hand"""
right_arm_position = poppy.l_arm_chain.end_effector + delta
poppy.r_arm_chain.goto(right_arm_position, 0.5, wait=True)
class Torso(object):
def __init__(self):
self.rospack = RosPack()
with open(join(self.rospack.get_path('nips2016'), 'config', 'torso.json')) as f:
self.params = json.load(f)
with open(join(self.rospack.get_path('nips2016'), 'config', 'perception.json')) as f:
self.perception_params = json.load(f) # TODO remove perception
self.execute_rate_hz = self.perception_params['recording_rate'] # TODO
self.execute_rate = rospy.Rate(self.execute_rate_hz)
self.publish_rate = rospy.Rate(self.params['publish_rate'])
self.eef_pub_l = rospy.Publisher('/nips2016/torso/left_arm/end_effector_pose', PoseStamped, queue_size=1)
self.eef_pub_r = rospy.Publisher('/nips2016/torso/right_arm/end_effector_pose', PoseStamped, queue_size=1)
self.js_pub_l = rospy.Publisher('/nips2016/torso/left_arm/joints', JointState, queue_size=1)
self.srv_reset = None
self.srv_execute = None
self.srv_set_compliant = None
self.primitive_head = None
self.primitive_right = None
# Protected resources
self.torso = None
self.in_rest_pose = False
self.robot_lock = RLock()
def go_to_rest(self, slow):
with self.robot_lock:
duration = 2 if slow else 0.25
self.set_torque_limits(60)
self.torso.goto_position({'l_shoulder_y': 13, 'l_shoulder_x': 20, 'l_elbow_y': -25}, duration)
rospy.sleep(duration)
self.go_to([90, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0], duration)
self.torso.motors[4].compliant = True # This motor overheats a lot
self.in_rest_pose = True
self.set_torque_limits()
def go_to(self, motors, duration):
motors_dict = dict(zip([m.name for m in self.torso.motors], motors))
self.torso.goto_position(motors_dict, duration)
rospy.sleep(duration)
def set_torque_limits(self, value=None):
for m in self.torso.l_arm:
m.torque_limit = self.params['torques'] if value is None else value
def run(self, dummy=False):
rospy.loginfo("Torso is connecting to the robot...")
try:
self.torso = PoppyTorso(use_http=True, simulator='poppy-simu' if dummy else None)
except IOError as e:
rospy.logerr("Torso failed to init: {}".format(e))
return None
else:
self.primitive_head = UpperBodyIdleMotion(self.torso, 15)
self.primitive_right = HeadIdleMotion(self.torso, 15)
self.go_to([90, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0], 4)
self.primitive_head.start()
self.primitive_right.start()
try:
self.set_torque_limits()
self.torso.compliant = False
self.go_to_rest(True)
self.srv_reset = rospy.Service('/nips2016/torso/reset', Reset, self._cb_reset)
self.srv_execute = rospy.Service('/nips2016/torso/execute', ExecuteTorsoTrajectory, self._cb_execute)
self.srv_set_compliant = rospy.Service('/nips2016/torso/set_compliant', SetTorsoCompliant, self._cb_set_compliant)
rospy.loginfo("Torso is ready to execute trajectories at {} Hz ".format(self.execute_rate_hz))
while not rospy.is_shutdown():
self.publish_eef(self.torso.l_arm_chain.end_effector, self.eef_pub_l)
self.publish_eef(self.torso.r_arm_chain.end_effector, self.eef_pub_r)
self.publish_js()
self.publish_rate.sleep()
finally:
self.primitive_right.stop()
self.primitive_head.stop()
self.torso.close()
def publish_eef(self, eef_pose, publisher):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'torso_base'
pose.pose.position.x = eef_pose[0]
pose.pose.position.y = eef_pose[1]
pose.pose.position.z = eef_pose[2]
publisher.publish(pose)
def publish_js(self):
js = JointState()
js.header.stamp = rospy.Time.now()
js.name = [m.name for m in self.torso.l_arm]
js.position = [m.present_position for m in self.torso.l_arm]
js.velocity = [m.present_speed for m in self.torso.l_arm]
js.effort = [m.present_load for m in self.torso.l_arm]
self.js_pub_l.publish(js)
def _cb_execute(self, request):
# TODO Action server
thread = Thread(target=self.execute, args=[request.torso_trajectory])
thread.daemon = True
thread.start()
return ExecuteTorsoTrajectoryResponse()
def execute(self, trajectory):
with self.robot_lock:
rospy.loginfo("Executing Torso trajectory with {} points...".format(len(trajectory.points)))
if not self.in_rest_pose:
self.go_to_rest(False)
for point in trajectory.points:
if rospy.is_shutdown():
break
self.torso.goto_position(dict(zip(trajectory.joint_names, point.positions)), 1.05/self.execute_rate_hz)
self.execute_rate.sleep()
rospy.loginfo("Trajectory ended!")
def _cb_set_compliant(self, request):
with self.robot_lock:
self.left_arm_compliant(request.compliant)
return SetTorsoCompliantResponse()
def left_arm_compliant(self, compliant):
rospy.loginfo("Torso left arm now {}".format('compliant' if compliant else 'rigid'))
for m in self.torso.l_arm:
m.compliant = compliant
def _cb_reset(self, request):
rospy.loginfo("Resetting Torso{}...".format(" in slow mode" if request.slow else ""))
if request.slow:
with self.robot_lock:
self.left_arm_compliant(False)
system('beep')
rospy.sleep(1)
self.go_to_rest(True)
else:
with self.robot_lock:
self.left_arm_compliant(False)
self.go_to_rest(False)
return ResetResponse()
import sys
import numpy as np
import time
import os
x = sys.argv[1]
y = sys.argv[2]
z = sys.argv[3]
nb_objet = sys.argv[4]
from poppy.creatures import PoppyTorso
from cinematique import Bras,Buste
from couple import *
#instanciation du robot avec le nom marlien (Mar pour Martin et Lien pour Julien)
marlien = PoppyTorso() #Commande uniquement pour le vrai robot
marlien.compliant = False
#mise à 0 des moteurs
for m in marlien.motors:
m.goto_position(0,2)
time.sleep(2)
coord_objet = np.mat([[float(x)],[float(y)],[float(z)]])
nb_objet = int(nb_objet)
marlien.l_shoulder_y.goto_position(-180,3)
marlien.l_elbow_y.goto_position(90,3)
marlien.r_shoulder_y.goto_position(-180,3)
marlien.r_elbow_y.goto_position(90,3,wait=True)
## Get the torso. from poppy.creatures import PoppyTorso from threading import Thread from pypot.server.httpserver import HTTPRobotServer torso = PoppyTorso() server = HTTPRobotServer(torso, host='193.50.110.242', port=8080) Thread(target=lambda: server.run(quiet=True, server='wsgiref')).start()
import time
import numpy as np
from poppy.creatures import PoppyTorso
poppy = PoppyTorso(simulator='vrep')
delay_time = 1
target_delta = np.array([-0.15, 0, 0])
# Initialize the robot
for m in poppy.motors:
m.goto_position(0, 2)
# Left arm is compliant, right arm is active
for m in poppy.l_arm:
m.compliant = False
for m in poppy.r_arm:
m.compliant = False
# The torso itself must not be compliant
for m in poppy.torso:
m.compliant = False
def follow_hand(poppy, delta):
"""Tell the right hand to follow the left hand"""
right_arm_position = poppy.l_arm_chain.end_effector + delta
poppy.r_arm_chain.goto(right_arm_position, 0.5, wait=True)
#!/usr/bin/env python # -*- coding: utf-8 -*- """ Created on Thu Feb 23 12:32:24 2017 @author: gdebat """ import json import pypot import time from pypot.vrep import from_vrep from poppy.creatures import PoppyTorso #from cherry import Cherry from STT_google import STT_google from chatterbot import sendToChatterbot #from textToSpeech.TTS_test import TTS_test poppy = PoppyTorso(simulator='vrep') #sp=Speak(poppy, text="Non vous n'avez pas souri!") sp = STT_google(poppy, state='normal') sp.run() print "ok" # poppy = PoppyTorso(simulator='vrep') # #sp=Speak(poppy, text="Non vous n'avez pas souri!") # sp=TTS_test(poppy, text='Bonjour', lang= 'fr', ttsengine='google') # sp.run() # print "ok"