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()
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()
