def __init__(self,
sync_sleep_time,
interpolation=False,
fraction_max_speed=0.01,
wait=False,
motor_config=None,
vrep=False,
vrep_host='127.0.0.1',
vrep_port=19997,
vrep_scene=None):
"""
The constructor of the class. Class properties should be set here
The robot handle should be created here
Any other initializations such as setting angles to particular values should also be taken care of here
:type sync_sleep_time: float
:param sync_sleep_time: Time to sleep to allow the joints to reach their targets (in seconds)
:type interpolation: bool
:param interpolation: Flag to indicate if intermediate joint angles should be interpolated
:type fractionMaxSpeed: float
:param fractionMaxSpeed: Fraction of the maximum motor speed to use
:type wait: bool
:param wait: Flag to indicate whether the control should wait for each angle to reach its target
"""
super(Poppy, self).__init__()
# Set the properties
self.sync_sleep_time = sync_sleep_time
self.interpolation = interpolation
self.fraction_max_speed = fraction_max_speed
self.wait = wait
self._maximumSpeed = 1.0
# Close existing vrep connections if any
pypot.vrep.close_all_connections()
# Create a new poppy robot and set the robot handle
self.robot_handle = PoppyHumanoid(simulator='vrep',
config=motor_config,
host=vrep_host,
port=vrep_port,
scene=vrep_scene)
# Sync the robot joints
self.robot_handle.start_sync()
# Perform required joint initializations
# Move arms to pi/2
# self.robot_handle.l_shoulder_y.goal_position = -90
# self.robot_handle.r_shoulder_y.goal_position = -90
# Sleep for a few seconds to allow the changes to take effect
time.sleep(3)
def __init__(self, next_position_x=0, next_position_y=0, position_difference_x=0,
position_difference_y=0, is_two_dimensional=False, final_movement=False, count=0):
self.count = count
self.poppy = PoppyHumanoid()
self.is_two_dimensional=is_two_dimensional
if self.is_two_dimensional is True:
if self.count is 0:
self.speed_obj = speed.Speed()
self.initialize_robot()
else:
self.position_difference_x = position_difference_x
self.position_difference_y = position_difference_y
self.final_movement = final_movement
self.next_position_y = next_position_x
self.next_position_y = next_position_y
self.speed_obj = speed.Speed()
else:
if self.count is 0:
self.initialize_robot()
else:
self.position_difference_x = position_difference_x
self.final_movement = final_movement
self.next_position_x = next_position_x
self.speed_obj = speed.Speed()
def __init__(self):
vrep.close_all_connections()
self.poppy = PoppyHumanoid(simulator='vrep')
self.waitTime()
#self.vrep_io = VrepIO(vrep_port=port, start=True)
#self.vrep_io.load_scene("thing.ttt")
#self.vrep_io.start_simulation()
self.poppy_head_pos = []
self.next_update = .2
self.last_update = time()
_ = system('clear')
def motorPosition(ID):
for motor in poppy.motors:
if ID == int(motor.id):
return int(motor.present_position)
while 1:
simulationCheck = raw_input(
'\n\nDeseja realizar uma simulacao v-rep[s] ou testes no humanoide[t]? (s/t)\n\n'
)
if simulationCheck == 's':
poppy = PoppyHumanoid(simulator='vrep')
break
elif simulationCheck == 't':
poppy = PoppyHumanoid()
break
else:
print('\nResposta invalida!\n')
clear()
IDs = []
for motor in poppy.motors:
IDs.append(motor.id)
print(IDs)
from pypot.creatures import PoppyHumanoid
poppy = PoppyHumanoid()
temp = ' '
poppy_servo_name = []
for i in poppy.motors:
for j in i.name:
con = j
con = con.encode('ascii','ignore')
if(con == 'x' or con == 'y' or con == 'z'):
temp = temp + con
poppy_servo_name.append(temp)
temp = ' '
else:
temp = temp + con
print(poppy_servo_name)
print(len(poppy_servo_name))
from pypot.creatures import PoppyHumanoid as PH
p = PH()
raw_input('Ready to turn off compliance before turning on?')
for m in p.motors:
m.compliant = False
# raw_input('Ready to make legs/arms/abs/chest compliant?')
# for rl in 'rl':
# for m in ['ankle', 'knee', 'hip']:
# getattr(p,'%s_%s_y'%(rl,m)).compliant = True
# for m in p.arms: m.compliant = True
# p.abs_y.compliant = True
# p.bust_y.compliant = True
raw_input('Ready to turn on compliance to get to bag pose?')
for m in p.motors:
m.compliant = True
raw_input('In bag pose and ready to turn off compliance again?')
for m in p.motors:
m.compliant = False
raw_input('Compliance should now be off.')
p.close()
from pypot.creatures import PoppyHumanoid as PH
import pickle as pkl
p = PH()
raw_input('Ready to turn off compliance and lift out of bag?')
for m in p.motors: m.compliant = False
raw_input('Ready to make legs compliant and put into sit position?')
for rl in 'rl':
for m in ['ankle', 'knee', 'hip']:
getattr(p,'%s_%s_y'%(rl,m)).compliant = True
raw_input('Ready to make hips/abs/chest/arms compliant and put to sit position?')
for rl in 'rl':
for m in ['ankle', 'knee']:
getattr(p,'%s_%s_y'%(rl,m)).compliant = False
for m in p.arms: m.compliant = True
p.abs_y.compliant = True
p.bust_y.compliant = True
raw_input('Ready to sit?')
with open("sit_angles.pkl","r") as f: sit_angles = pkl.load(f)
for m in p.motors: m.compliant = False
p.goto_position(sit_angles, 5, wait=True)
from pypot.creatures import PoppyHumanoid as PH
from pypot.sensor import OpenCVCamera
import pickle as pk
from record_angles import hotkeys
import time
import matplotlib.pyplot as pt
import numpy as np
with open("crawl2.pkl", "r") as f:
crawl_angles = pk.load(f)
fps = 1
c = OpenCVCamera("poppy-cam", 0, fps)
p = PH()
key = raw_input("reposition arms? [y/n]")
if key == "y":
for arm in ["left", "right"]:
raw_input('Enter for %s arm compliance...' % arm)
motors = [m for m in p.arms if m.name[0] == arm[0]]
for m in motors:
m.compliant = True
while True:
raw_input('Enter for %s arm noncompliant...' % arm)
pos = getattr(p, arm[0] + "_shoulder_y").present_position
if pos < -90 or pos > 90:
print("shoulder_y pos=%f not in [-90..90]!" % pos)
else:
for m in motors:
m.compliant = False
break
from pypot.sensor import OpenCVCamera
import matplotlib.pyplot as plt
import time
import numpy as np
import sys
jlims = {
"head_y": [-22., 20.], # backward/forward
"head_z": [-58., 40.], # right/left
"l_ankle_y": [-40, 40], # flex/point
"r_ankle_y": [-40, 40], # flex/point
}
fps = 10
# moving_speed = 25
p = PH()
for m in jlims.keys():
getattr(p, m).compliant = False
# getattr(p,m).moving_speed = moving_speed
p.goto_position({"r_ankle_y": -40, "l_ankle_y": 40}, 1, wait=True)
c = OpenCVCamera("poppy-cam", 0, fps)
def handle_close(event):
c.close()
p.close()
sys.exit(0)
class Poppy(Robot):
"""
This class encapsulates the methods and properties for interacting with the poppy robot
It extends the abstract class 'Robot'
"""
sync_sleep_time = None
robot_handle = None
interpolation = None
fraction_max_speed = None
wait = None
def __init__(self,
sync_sleep_time,
interpolation=False,
fraction_max_speed=0.01,
wait=False,
motor_config=None,
vrep=False,
vrep_host='127.0.0.1',
vrep_port=19997,
vrep_scene=None):
"""
The constructor of the class. Class properties should be set here
The robot handle should be created here
Any other initializations such as setting angles to particular values should also be taken care of here
:type sync_sleep_time: float
:param sync_sleep_time: Time to sleep to allow the joints to reach their targets (in seconds)
:type interpolation: bool
:param interpolation: Flag to indicate if intermediate joint angles should be interpolated
:type fractionMaxSpeed: float
:param fractionMaxSpeed: Fraction of the maximum motor speed to use
:type wait: bool
:param wait: Flag to indicate whether the control should wait for each angle to reach its target
"""
super(Poppy, self).__init__()
# Set the properties
self.sync_sleep_time = sync_sleep_time
self.interpolation = interpolation
self.fraction_max_speed = fraction_max_speed
self.wait = wait
self._maximumSpeed = 1.0
# Close existing vrep connections if any
pypot.vrep.close_all_connections()
# Create a new poppy robot and set the robot handle
self.robot_handle = PoppyHumanoid(simulator='vrep',
config=motor_config,
host=vrep_host,
port=vrep_port,
scene=vrep_scene)
# Sync the robot joints
self.robot_handle.start_sync()
# Perform required joint initializations
# Move arms to pi/2
# self.robot_handle.l_shoulder_y.goal_position = -90
# self.robot_handle.r_shoulder_y.goal_position = -90
# Sleep for a few seconds to allow the changes to take effect
time.sleep(3)
def set_angles(self, joint_angles, duration=None, joint_velocities=None):
"""
Sets the joints to the specified angles (after converting radians to degrees since the poppy robot uses degrees)
:type joint_angles: dict
:param joint_angles: Dictionary of joint_names: angles (in radians)
:type duration: float
:param duration: Time to reach the angular targets (in seconds)
:type joint_velocities: dict
:param joint_velocities: dict of joint angles and velocities
:return: None
"""
for joint_name in joint_angles.keys():
# Convert the angle to degrees
target_angle_degrees = math.degrees(joint_angles[joint_name])
try:
# Determine the right joint and set the joint angle
for motor in self.robot_handle.motors:
if motor.name == joint_name:
motor.compliant = False
motor.goal_speed = 1000.0 * min(
self.fraction_max_speed, self._maximumSpeed)
motor.goal_position = target_angle_degrees
break
except Exception as e: # Catch all exceptions
print e.message
raise RuntimeError('Could not set joint angle')
# Sleep to allow the motors to reach their targets
if not duration:
time.sleep(self.sync_sleep_time)
def get_angles(self, joint_names=None):
"""
Gets the angles of the specified joints and returns a dict of joint_names: angles (in radians)
If joint_names=None then the values of all joints are returned
:type joint_names: list
:param joint_names: List of joint name strings
:rtype: dict
:returns: dict of joint names and angles
"""
# Create the dict to be returned
joint_angles = dict()
# Retrieve the list of DxlMXMotor objects
motors = self.robot_handle.motors
# If no joint names are specified, return angles of all joints in raidans
# Else return only the angles (in radians) of the specified joints
for m in motors:
if joint_names is None:
joint_angles[str(m.name)] = math.radians(m.present_position)
else:
if m.name in joint_names:
joint_angles[str(m.name)] = math.radians(
m.present_position)
return joint_angles
def cleanup(self):
"""
Cleans up the current connection to the robot
:return: None
"""
# TODO check if it works
self.robot_handle.close()
class PoppySim(object):
def __init__(self):
vrep.close_all_connections()
self.poppy = PoppyHumanoid(simulator='vrep')
self.waitTime()
#self.vrep_io = VrepIO(vrep_port=port, start=True)
#self.vrep_io.load_scene("thing.ttt")
#self.vrep_io.start_simulation()
self.poppy_head_pos = []
self.next_update = .2
self.last_update = time()
#self.motor_names_file = open(motorfile, 'r')
#self.motor_names = []
def waitTime(self):
t0 = time()
while time() - t0 <= .1:
pass
def getOrientation(self):
#self.poppy_head_pos = self.vrep_io.get_object_orientation("head_visual")
self.poppy_head_pos = self.poppy.get_object_orientation("head_visual")
self.waitTime()
self.poppy_Lthigh_orientation = self.poppy.get_object_orientation(
"l_thigh_visual")
self.waitTime()
self.poppy_Rthigh_orientation = self.poppy.get_object_orientation(
"r_thigh_visual")
self.waitTime()
concant_list = self.poppy_head_pos + self.poppy_Lthigh_orientation + self.poppy_Rthigh_orientation
return concant_list
#def populateMotorNames(self):
#for line in self.motor_names_file:
#self.motor_names.append(line)
#pass
def nnOut(self, nn):
if time() - self.last_update >= self.next_update:
o = self.getOrientation()
for x in o:
x /= 360
o = nn.forward((numpy.asarray(o)).reshape((1, 9)))
ol = []
for x in numpy.nditer(o):
ol.append(float(x))
for x, y in zip(ol, self.poppy.motors):
y.goal_position = float(x * 360)
if y.goal_position >= 360:
print("goal position out of range!")
#for x,y in zip(ol, self.motor_names):
#self.vrep_io.set_motor_position(y,x)
def fitness(self):
f = -self.poppy.get_object_position("pelvis_visual")[1]
self.t0 = time()
self.poppy.reset_simulation()
return f
def runSim(self, nn):
self.t0 = time()
self.poppy.start_simulation()
while (time() - self.t0) <= 8.0:
self.nnOut(nn)
return self.fitness()
