def __init__(self, env = None):
if env == None:
self.env = openravepy.Environment()
self.robot = self.env.ReadRobotXMLFile("robots/pr2-beta-sim.robot.xml")
self.env.Add(self.robot)
elif len(env.GetRobots())>0:
self.env = env
self.robot = env.GetRobots()[0]
else:
rospy.loginfo("Valid env not found")
sys.exit()
rospy.loginfo("Loading IK for rightarm")
self.robot.SetActiveManipulator("rightarm")
self.rightarm_ik = InverseKinematicsModel(self.robot,
iktype=openravepy.IkParameterization.Type.Transform6D)
if not self.rightarm_ik.load():
self.rightarm_ik.autogenerate()
rospy.loginfo("Loading IK for leftarm")
self.robot.SetActiveManipulator("leftarm")
self.leftarm_ik = InverseKinematicsModel(self.robot,
iktype=openravepy.IkParameterization.Type.Transform6D)
if not self.leftarm_ik.load():
self.leftarm_ik.autogenerate()
self.robot_state = pr2_control_utilities.RobotState()
self.controller = pr2_control_utilities.PR2JointMover(robot_state = self.robot_state,
name = "PR2 Controller",
time_to_reach=5.0
)
self.listener = tf.TransformListener()
#fixing the joints
joint_msg = self.robot_state.last_joint_msg
ros_names = joint_msg.name
inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
# make the joint limits match the PR2 soft limits
low_limits, high_limits = self.robot.GetDOFLimits()
rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(),
rarm_low_limits, rarm_high_limits):
if low is not None and high is not None:
low_limits[rarm_index] = low
high_limits[rarm_index] = high
larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(),
larm_low_limits, larm_high_limits):
if low is not None and high is not None:
low_limits[larm_index] = low
high_limits[larm_index] = high
self.robot.SetDOFLimits(low_limits, high_limits)
def _SetupIK(self, iktype):
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
iktype=iktype)
if not self.ikmodel.load():
self.ikmodel.generate(iktype=iktype, precision=4,
freeindices=[ self.GetIndices()[2] ])
self.ikmodel.save()
def load_ikfast(self, freeinc=np.pi / 6.):
success = False
from openravepy.databases.inversekinematics import InverseKinematicsModel
manipulators = [self.left_manip, self.right_manip]
if None not in manipulators:
for manip in manipulators:
ikmodel = InverseKinematicsModel(self.robot,
iktype=self.iktype,
manip=manip)
success = ikmodel.load(freeinc=[freeinc])
if not success:
break
return success
def _SetupIK(self, iktype):
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot, manip=self, iktype=iktype)
if not self.ikmodel.load():
self.ikmodel.generate(iktype=iktype, precision=4, freeindices=[self.GetIndices()[2]])
self.ikmodel.save()
def _SetupIK(self, iktype):
""" creates Ikfast file for chosen manipulator
"""
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot,
manip=self,
iktype=iktype)
if not self.ikmodel.load():
if (self.namespace == 'arm'):
self.ikmodel.generate(iktype=iktype,
precision=4,
freeindices=[self.GetIndices()[2]])
self.ikmodel.save()
else:
self.ikmodel.generate(
iktype=iktype,
precision=4,
freeindices=[self.GetIndices()[4],
self.GetIndices()[3]])
self.ikmodel.save()
def load_ikfast(robot,
iktype,
freejoints=['J6'],
freeinc=[0.01],
autogenerate=True):
"""
Load the IKFast solver
Parameters
----------
robot: orpy.Robot
The OpenRAVE robot
iktype: orpy.IkParameterizationType
Inverse kinematics type to be used
freeinc: list
The increment (discretization) to be used for the free DOF when the target
`iktype` is `TranslationDirection5D`
autogenerate: bool, optional
If true, auto-generate the IKFast solver
Returns
-------
success: bool
`True` if succeeded, `False` otherwise
"""
# Improve code readability
from openravepy.databases.inversekinematics import InverseKinematicsModel
# Initialize the ikmodel
if iktype == orpy.IkParameterizationType.TranslationDirection5D:
ikmodel = InverseKinematicsModel(robot,
iktype=iktype,
freejoints=freejoints)
elif iktype == orpy.IkParameterizationType.Transform6D:
ikmodel = InverseKinematicsModel(robot, iktype=iktype)
else:
raise TypeError('Unsupported ikmodel: {}'.format(iktype.name))
# Load or generate
if not ikmodel.load() and autogenerate:
print 'Generating IKFast {0}. Will take few minutes...'.format(
iktype.name)
if iktype == orpy.IkParameterizationType.Transform6D:
ikmodel.autogenerate()
elif iktype == orpy.IkParameterizationType.TranslationDirection5D:
ikmodel.generate(iktype=iktype, freejoints=freejoints)
ikmodel.save()
else:
ikmodel.autogenerate()
print 'IKFast {0} has been successfully generated'.format(iktype.name)
if iktype == orpy.IkParameterizationType.TranslationDirection5D:
success = ikmodel.load(freeinc=freeinc)
elif iktype == orpy.IkParameterizationType.Transform6D:
success = ikmodel.load()
return success
class PR2Robot(object):
def __init__(self, env = None):
if env == None:
self.env = openravepy.Environment()
self.robot = self.env.ReadRobotXMLFile("robots/pr2-beta-sim.robot.xml")
self.env.Add(self.robot)
elif len(env.GetRobots())>0:
self.env = env
self.robot = env.GetRobots()[0]
else:
rospy.loginfo("Valid env not found")
sys.exit()
rospy.loginfo("Loading IK for rightarm")
self.robot.SetActiveManipulator("rightarm")
self.rightarm_ik = InverseKinematicsModel(self.robot,
iktype=openravepy.IkParameterization.Type.Transform6D)
if not self.rightarm_ik.load():
self.rightarm_ik.autogenerate()
rospy.loginfo("Loading IK for leftarm")
self.robot.SetActiveManipulator("leftarm")
self.leftarm_ik = InverseKinematicsModel(self.robot,
iktype=openravepy.IkParameterization.Type.Transform6D)
if not self.leftarm_ik.load():
self.leftarm_ik.autogenerate()
self.robot_state = pr2_control_utilities.RobotState()
self.controller = pr2_control_utilities.PR2JointMover(robot_state = self.robot_state,
name = "PR2 Controller",
time_to_reach=5.0
)
self.listener = tf.TransformListener()
#fixing the joints
joint_msg = self.robot_state.last_joint_msg
ros_names = joint_msg.name
inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
# make the joint limits match the PR2 soft limits
low_limits, high_limits = self.robot.GetDOFLimits()
rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(),
rarm_low_limits, rarm_high_limits):
if low is not None and high is not None:
low_limits[rarm_index] = low
high_limits[rarm_index] = high
larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(),
larm_low_limits, larm_high_limits):
if low is not None and high is not None:
low_limits[larm_index] = low
high_limits[larm_index] = high
self.robot.SetDOFLimits(low_limits, high_limits)
def convertPose(self, pose):
assert isinstance(pose, PoseStamped)
self.listener.waitForTransform("/base_footprint", pose.header.frame_id,
rospy.Time.now(), rospy.Duration(1))
newpose = self.listener.transformPose("/base_footprint", pose)
translation = tf.listener.xyz_to_mat44(newpose.pose.position),
orientation = tf.listener.xyzw_to_mat44(newpose.pose.orientation)
matrix4 = np.dot(translation, orientation).squeeze()
return matrix4
def __ik_solution(self, pose, manip, end_effector_link, ignore_end_effector=True,
multiple_soluitions = False):
T = self.convertPose(pose)
self.update_rave()
worldFromEE = utils.tf_for_link(T, manip, end_effector_link)
filter_options = openravepy.IkFilterOptions.CheckEnvCollisions
if ignore_end_effector:
filter_options = filter_options | openravepy.IkFilterOptions.IgnoreEndEffectorCollisions
if multiple_soluitions:
sol = manip.FindIKSolutions(worldFromEE, filter_options)
else:
sol = manip.FindIKSolution(worldFromEE, filter_options)
return sol
def find_rightarm_ik(self, pose, ignore_end_effector=True,
multiple_soluitions = False):
manip = self.robot.SetActiveManipulator("rightarm")
return self.__ik_solution(pose, manip, "r_wrist_roll_link",
ignore_end_effector,
multiple_soluitions)
def find_leftarm_ik(self, pose, ignore_end_effector=True,
multiple_soluitions = False):
manip = self.robot.SetActiveManipulator("leftarm")
return self.__ik_solution(pose, manip, "l_wrist_roll_link",
ignore_end_effector,
multiple_soluitions)
def move_right_arm(self, pose, ignore_end_effector = True):
sol = self.find_rightarm_ik(pose, ignore_end_effector)
if sol is None:
rospy.logerr("Could not find an IK solution!")
return False
self.controller.set_arm_state(sol.tolist(), "right", True)
self.update_rave()
return True
def move_left_arm(self, pose, ignore_end_effector = True):
sol = self.find_leftarm_ik(pose, ignore_end_effector)
if sol is None:
rospy.logerr("Could not find an IK solution!")
return False
self.controller.set_arm_state(sol.tolist(), "left", True)
self.update_rave()
return True
def update_rave(self):
ros_values = self.robot_state.last_joint_msg.position
rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]
self.robot.SetJointValues(rave_values[:20],self.rave_inds[:20])
self.robot.SetJointValues(rave_values[20:],self.rave_inds[20:])
def tuck_left_arm(self):
self.controller.set_arm_state(pr2_l_arm_tucked, "left", True)
self.update_rave()
def tuck_right_arm(self):
self.controller.set_arm_state(pr2_r_arm_tucked, "right", True)
self.update_rave()
def tuck_arms(self):
self.tuck_left_arm()
self.tuck_right_arm()
class WAM(Manipulator):
def __init__(self, sim, namespace='',
iktype=openravepy.IkParameterization.Type.Transform6D):
Manipulator.__init__(self)
self.simulated = sim
self._iktype = iktype
self.namespace = namespace
if iktype is not None:
self._SetupIK(iktype)
if sim:
from prpy.simulation import ServoSimulator
self.servo_simulator = ServoSimulator(
self, rate=20, watchdog_timeout=0.1)
def IsSimulated(self):
return self.simulated
def GetJointNames(self):
return [self.namespace + '/' + j for j in
['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'j7']]
def CloneBindings(self, parent):
Manipulator.CloneBindings(self, parent)
self.simulated = True
self._iktype = parent._iktype
if self._iktype is not None:
self._SetupIK(self._iktype)
def _SetupIK(self, iktype):
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
iktype=iktype)
if not self.ikmodel.load():
self.ikmodel.generate(iktype=iktype, precision=4,
freeindices=[ self.GetIndices()[2] ])
self.ikmodel.save()
def SetStiffness(self, stiffness):
"""Set the WAM's stiffness.
Stiffness False/0 is gravity compensation and stiffness True/1 is position
control. Values between 0 and 1 are experimental.
@param stiffness boolean or decimal value between 0.0 and 1.0
"""
self.GetRobot().SetStiffness(stiffness, manip=self)
def SetTrajectoryExecutionOptions(self, traj, stop_on_stall=False,
stop_on_ft=False, force_magnitude=None, force_direction=None,
torque=None):
"""Set OWD's trajectory execution options on trajectory.
@param stop_on_stall aborts the trajectory if the arm stalls
@param stop_on_ft aborts the trajectory if the force/torque
sensor reports a force or torque that exceeds
threshold specified by the force_magnitude,
force_direction, and torque options
@param force_magnitude force threshold value, in Newtons
@param force_direction unit vector in the force/torque coordinate
frame, which is int he same orientation as the
hand frame.
@param torque vector of the three torques
"""
raise NotImplementedError('OWD execution options not supported under ros_control')
def Servo(self, velocities):
"""Servo with a vector of instantaneous joint velocities.
@param velocities joint velocities, in radians per second
"""
num_dof = len(self.GetArmIndices())
if len(velocities) != num_dof:
raise ValueError('Incorrect number of joint velocities. '
'Expected {0:d}; got {0:d}.'.format(
num_dof, len(velocities)))
if not self.simulated:
raise NotImplementedError('Servoing and velocity control not yet'
'supported under ros_control.')
else:
self.servo_simulator.SetVelocity(velocities)
def ServoTo(self, target, duration, timeStep=0.05, collisionChecking=True):
"""Servo the arm towards a target configuration over some duration.
Servos the arm towards a target configuration with a constant joint
velocity. This function uses the \ref Servo command to control the arm
and must be called repeatidly until the arm reaches the goal. If \tt
collisionChecking is enabled, then the servo will terminate and return
False if a collision is detected with the simulation environment.
@param target desired configuration
@param duration duration in seconds
@param timestep period of the control loop, in seconds
@param collisionchecking check collisions in the simulation environment
@return whether the servo was successful
"""
steps = int(math.ceil(duration/timeStep))
original_dofs = self.GetRobot().GetDOFValues(self.GetArmIndices())
velocity = numpy.array(target - self.GetRobot().GetDOFValues(self.GetArmIndices()))
velocities = [v/steps for v in velocity]
inCollision = False
if collisionChecking:
inCollision = self.CollisionCheck(target)
if not inCollision:
for i in range(1,steps):
import time
self.Servo(velocities)
time.sleep(timeStep)
self.Servo([0] * len(self.GetArmIndices()))
# new_dofs = self.GetRobot().GetDOFValues(self.GetArmIndices())
return True
else:
return False
def GetVelocityLimits(self, openrave=None, owd=None):
"""Get the OpenRAVE and OWD joint velocity limits.
This function checks both the OpenRAVE and OWD joint velocity limits.
If they do not match, a warning is printed and the minimum value is
returned.
@param openrave flag to set the OpenRAVE velocity limits
@param owd flag to set the OWD velocity limits
@return list of velocity limits, in radians per second
"""
if openrave is not None or owd is not None:
warnings.warn(
'The "openrave" and "owd" flags are deprecated in'
' GetVelocityLimits and will be removed in a future version.',
DeprecationWarning)
return Manipulator.GetVelocityLimits(self)
def SetVelocityLimits(self, velocity_limits, min_accel_time,
openrave=True, owd=None):
"""Change the OpenRAVE and OWD joint velocity limits.
Joint velocities that exceed these limits will trigger a velocity fault.
@param velocity_limits vector of joint velocity limits in radians per second
@param min_accel_time minimum acceleration time used to compute acceleration limits
@param openrave flag to set the OpenRAVE velocity limits
@param owd flag to set the OWD velocity limits
"""
# TODO implement in ros_control
if owd is not None:
warnings.warn(
'The "owd" flag is deprecated in'
' SetVelocityLimits and will be removed in a future version.',
DeprecationWarning)
# Update the OpenRAVE limits.
if openrave:
Manipulator.SetVelocityLimits(self, velocity_limits, min_accel_time)
def GetTrajectoryStatus(manipulator):
"""Gets the status of the current (or previous) trajectory executed by OWD.
@return status of the current (or previous) trajectory executed
"""
raise NotImplementedError('GetTrajectoryStatus not supported on manipulator.'
' Use returned TrajectoryFuture instead.')
def ClearTrajectoryStatus(manipulator):
"""Clears the current trajectory execution status.
This resets the output of \ref GetTrajectoryStatus.
"""
raise NotImplementedError('ClearTrajectoryStatus not supported on manipulator.')
def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
max_force=5.0, max_torque=None, ignore_collisions=None,
velocity_limit_scale=0.25, **kw_args):
"""Execute a straight move-until-touch action.
This action stops when a sufficient force is is felt or the manipulator
moves the maximum distance. The motion is considered successful if the
end-effector moves at least distance. In simulation, a move-until-touch
action proceeds until the end-effector collids with the environment.
@param direction unit vector for the direction of motion in the world frame
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param max_force maximum force in Newtons
@param max_torque maximum torque in Newton-Meters
@param ignore_collisions collisions with these objects are ignored when
planning the path, e.g. the object you think you will touch
@param velocity_limit_scale A multiplier to use to scale velocity limits
when executing MoveUntilTouch ( < 1 in most cases).
@param **kw_args planner parameters
@return felt_force flag indicating whether we felt a force.
"""
from contextlib import nested
from openravepy import CollisionReport, KinBody, Robot, RaveCreateTrajectory
from prpy.planning.exceptions import CollisionPlanningError
delta_t = 0.01
robot = manipulator.GetRobot()
env = robot.GetEnv()
dof_indices = manipulator.GetArmIndices()
direction = numpy.array(direction, dtype='float')
# Default argument values.
if max_distance is None:
max_distance = 1.
warnings.warn(
'MoveUntilTouch now requires the "max_distance" argument.'
' This will be an error in the future.',
DeprecationWarning)
if max_torque is None:
max_torque = numpy.array([100.0, 100.0, 100.0])
if ignore_collisions is None:
ignore_collisions = []
with env:
# Compute the expected force direction in the hand frame.
hand_pose = manipulator.GetEndEffectorTransform()
force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)
# Disable the KinBodies listed in ignore_collisions. We backup the
# "enabled" state of all KinBodies so we can restore them later.
body_savers = [
body.CreateKinBodyStateSaver() for body in ignore_collisions]
robot_saver = robot.CreateRobotStateSaver(
Robot.SaveParameters.ActiveDOF
| Robot.SaveParameters.ActiveManipulator
| Robot.SaveParameters.LinkTransformation)
with robot_saver, nested(*body_savers) as f:
manipulator.SetActive()
robot_cspec = robot.GetActiveConfigurationSpecification()
for body in ignore_collisions:
body.Enable(False)
path = robot.PlanToEndEffectorOffset(direction=direction,
distance=distance, max_distance=max_distance, **kw_args)
# Execute on the real robot by tagging the trajectory with options that
# tell the controller to stop on force/torque input.
if not manipulator.simulated:
raise NotImplementedError('MoveUntilTouch not yet implemented under ros_control.')
# Forward-simulate the motion until it hits an object.
else:
traj = robot.PostProcessPath(path)
is_collision = False
traj_cspec = traj.GetConfigurationSpecification()
new_traj = RaveCreateTrajectory(env, '')
new_traj.Init(traj_cspec)
robot_saver = robot.CreateRobotStateSaver(
Robot.SaveParameters.LinkTransformation)
with env, robot_saver:
for t in numpy.arange(0, traj.GetDuration(), delta_t):
waypoint = traj.Sample(t)
dof_values = robot_cspec.ExtractJointValues(
waypoint, robot, dof_indices, 0)
manipulator.SetDOFValues(dof_values)
# Terminate if we detect collision with the environment.
report = CollisionReport()
if env.CheckCollision(robot, report=report):
logger.info('Terminated from collision: %s',
str(CollisionPlanningError.FromReport(report)))
is_collision = True
break
elif robot.CheckSelfCollision(report=report):
logger.info('Terminated from self-collision: %s',
str(CollisionPlanningError.FromReport(report)))
is_collision = True
break
# Build the output trajectory that stops in contact.
if new_traj.GetNumWaypoints() == 0:
traj_cspec.InsertDeltaTime(waypoint, 0.)
else:
traj_cspec.InsertDeltaTime(waypoint, delta_t)
new_traj.Insert(new_traj.GetNumWaypoints(), waypoint)
if new_traj.GetNumWaypoints() > 0:
robot.ExecuteTrajectory(new_traj)
return is_collision
class WAM(Manipulator):
def __init__(self, sim, owd_namespace,
iktype=openravepy.IkParameterization.Type.Transform6D):
Manipulator.__init__(self)
self.simulated = sim
self._iktype = iktype
if iktype is not None:
self._SetupIK(iktype)
# Enable servo motions in simulation mode.
self.controller = self.GetRobot().AttachController(name=self.GetName(),
args='OWDController {0:s} {1:s}'.format('prpy', owd_namespace),
dof_indices=self.GetArmIndices(), affine_dofs=0, simulated=sim
)
if sim:
from prpy.simulation import ServoSimulator
self.servo_simulator = ServoSimulator(
self, rate=20, watchdog_timeout=0.1)
def CloneBindings(self, parent):
Manipulator.CloneBindings(self, parent)
self.simulated = True
self.controller = None
self._iktype = parent._iktype
if self._iktype is not None:
self._SetupIK(self._iktype)
def _SetupIK(self, iktype):
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
iktype=iktype)
if not self.ikmodel.load():
self.ikmodel.generate(iktype=iktype, precision=4,
freeindices=[ self.GetIndices()[2] ])
self.ikmodel.save()
def SetStiffness(manipulator, stiffness):
"""Set the WAM's stiffness.
Stiffness 0 is gravity compensation and stiffness 1 is position
control. Values between 0 and 1 are experimental.
@param stiffness value between 0.0 and 1.0
"""
if not (0 <= stiffness <= 1):
raise Exception('Stiffness must in the range [0, 1]; got %f.' % stiffness)
if not manipulator.simulated:
manipulator.controller.SendCommand('SetStiffness {0:f}'.format(stiffness))
def SetTrajectoryExecutionOptions(self, traj, stop_on_stall=False,
stop_on_ft=False, force_magnitude=None, force_direction=None,
torque=None):
"""Set OWD's trajectory execution options on trajectory.
@param stop_on_stall aborts the trajectory if the arm stalls
@param stop_on_ft aborts the trajectory if the force/torque
sensor reports a force or torque that exceeds
threshold specified by the force_magnitude,
force_direction, and torque options
@param force_magnitude force threshold value, in Newtons
@param force_direction unit vector in the force/torque coordinate
frame, which is int he same orientation as the
hand frame.
@param torque vector of the three torques
"""
util.SetTrajectoryTags(traj, {
'owd_options': {
'stop_on_stall': bool(stop_on_stall),
'stop_on_ft': bool(stop_on_ft),
'force_magnitude': float(force_magnitude),
'force_direction': list(force_direction),
'torque': list(torque),
}}, append=True)
def Servo(manipulator, velocities):
"""Servo with a vector of instantaneous joint velocities.
@param velocities joint velocities, in radians per second
"""
num_dof = len(manipulator.GetArmIndices())
if len(velocities) != num_dof:
raise ValueError('Incorrect number of joint velocities. '
'Expected {0:d}; got {0:d}.'.format(
num_dof, len(velocities)))
if not manipulator.simulated:
manipulator.controller.SendCommand('Servo ' + ' '.join([ str(qdot) for qdot in velocities ]))
else:
manipulator.controller.Reset(0)
manipulator.servo_simulator.SetVelocity(velocities)
def ServoTo(manipulator, target, duration, timeStep=0.05, collisionChecking=True):
"""Servo the arm towards a target configuration over some duration.
Servos the arm towards a target configuration with a constant joint
velocity. This function uses the \ref Servo command to control the arm
and must be called repeatidly until the arm reaches the goal. If \tt
collisionChecking is enabled, then the servo will terminate and return
False if a collision is detected with the simulation environment.
@param target desired configuration
@param duration duration in seconds
@param timestep period of the control loop, in seconds
@param collisionchecking check collisions in the simulation environment
@return whether the servo was successful
"""
steps = int(math.ceil(duration/timeStep))
original_dofs = manipulator.GetRobot().GetDOFValues(manipulator.GetArmIndices())
velocity = numpy.array(target-manipulator.GetRobot().GetDOFValues(manipulator.GetArmIndices()))
velocities = v/steps#[v/steps for v in velocity]
inCollision = False
if collisionChecking:
inCollision = manipulator.CollisionCheck(target)
if not inCollision:
for i in range(1,steps):
manipulator.Servo(velocities)
time.sleep(timeStep)
manipulator.Servo([0] * len(manipulator.GetArmIndices()))
new_dofs = manipulator.GetRobot().GetDOFValues(manipulator.GetArmIndices())
return True
else:
return False
def GetVelocityLimits(self, openrave=True, owd=True):
"""Get the OpenRAVE and OWD joint velocity limits.
This function checks both the OpenRAVE and OWD joint velocity limits.
If they do not match, a warning is printed and the minimum value is
returned.
@param openrave flag to set the OpenRAVE velocity limits
@param owd flag to set the OWD velocity limits
@return list of velocity limits, in radians per second
"""
# Update the OpenRAVE limits.
if openrave:
or_velocity_limits = Manipulator.GetVelocityLimits(self)
if self.simulated or not owd:
return or_velocity_limits
# Update the OWD limits.
if owd and not self.simulated:
args = [ 'GetSpeed' ]
args_str = ' '.join(args)
owd_speed_limits_all = self.controller.SendCommand(args_str)
#if we get nothing back, e.g. if the arm isn't running, return openrave lims
if owd_speed_limits_all is None:
return or_velocity_limits
owd_speed_limits = map(float, owd_speed_limits_all.split(','));
#first 7 numbers are velocity limits
owd_velocity_limits = numpy.array(owd_speed_limits[0:len(self.GetIndices())])
diff_arr = numpy.subtract(or_velocity_limits, owd_velocity_limits)
max_diff = max(abs(diff_arr))
if max_diff > 0.01:
# TODO: Change this to use the logging framework.
print('GetVelocityLimits Error: openrave and owd limits very different')
print('\tOpenrave limits:\t' + str(or_velocity_limits))
print('\tOWD limits:\t\t' + str(owd_velocity_limits))
return numpy.minimum(or_velocity_limits, owd_velocity_limits)
return or_velocity_limits
def SetVelocityLimits(self, velocity_limits, min_accel_time,
openrave=True, owd=True):
"""Change the OpenRAVE and OWD joint velocity limits.
Joint velocities that exceed these limits will trigger a velocity fault.
@param velocity_limits vector of joint velocity limits in radians per second
@param min_accel_time minimum acceleration time used to compute acceleration limits
@param openrave flag to set the OpenRAVE velocity limits
@param owd flag to set the OWD velocity limits
"""
# Update the OpenRAVE limits.
if openrave:
Manipulator.SetVelocityLimits(self, velocity_limits, min_accel_time)
# Update the OWD limits.
if owd and not self.simulated:
args = [ 'SetSpeed' ]
args += [ str(min_accel_time) ]
args += [ str(velocity) for velocity in velocity_limits ]
args_str = ' '.join(args)
self.controller.SendCommand(args_str)
def GetTrajectoryStatus(manipulator):
"""Gets the status of the current (or previous) trajectory executed by OWD.
@return status of the current (or previous) trajectory executed
"""
if not manipulator.simulated:
return manipulator.controller.SendCommand('GetStatus')
else:
if manipulator.controller.IsDone():
return 'done'
else:
return 'active'
def ClearTrajectoryStatus(manipulator):
"""Clears the current trajectory execution status.
This resets the output of \ref GetTrajectoryStatus.
"""
if not manipulator.simulated:
manipulator.controller.SendCommand('ClearStatus')
def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
max_force=5.0, max_torque=None, ignore_collisions=None, **kw_args):
"""Execute a straight move-until-touch action.
This action stops when a sufficient force is is felt or the manipulator
moves the maximum distance. The motion is considered successful if the
end-effector moves at least distance. In simulation, a move-until-touch
action proceeds until the end-effector collids with the environment.
@param direction unit vector for the direction of motion in the world frame
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param max_force maximum force in Newtons
@param max_torque maximum torque in Newton-Meters
@param execute optionally execute the trajectory
@param ignore_collisions collisions with these objects are ignored in simulation
@param **kw_args planner parameters
@return felt_force flag indicating whether we felt a force.
"""
# TODO: Is ignore_collisions a list of names or KinBody pointers?
if max_torque is None:
max_torque = numpy.array([100.0, 100.0, 100.0 ])
ignore_col_obj_oldstate = []
if ignore_collisions is None:
ignore_collisions = []
for ignore_col_with in ignore_collisions:
ignore_col_obj_oldstate.append(ignore_col_with.IsEnabled())
ignore_col_with.Enable(False)
with manipulator.GetRobot().GetEnv():
manipulator.GetRobot().GetController().SimulationStep(0)
# Compute the expected force direction in the hand frame.
direction = numpy.array(direction)
hand_pose = manipulator.GetEndEffectorTransform()
force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)
with manipulator.GetRobot():
old_active_manipulator = manipulator.GetRobot().GetActiveManipulator()
manipulator.SetActive()
traj = manipulator.PlanToEndEffectorOffset(direction, distance, max_distance=max_distance,
execute=False, **kw_args)
collided_with_obj = False
try:
if not manipulator.simulated:
manipulator.SetTrajectoryExecutionOptions(traj, stop_on_ft=True,
force_direction=force_direction, force_magnitude=max_force,
torque=max_torque)
manipulator.hand.TareForceTorqueSensor()
manipulator.GetRobot().ExecutePath(traj)
for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
ignore_col_with.Enable(oldstate)
else:
traj_duration = traj.GetDuration()
delta_t = 0.01
traj_config_spec = traj.GetConfigurationSpecification()
traj_angle_group = traj_config_spec.GetGroupFromName('joint_values')
path_config_spec = openravepy.ConfigurationSpecification()
# path_config_spec.AddDeltaTimeGroup()
path_config_spec.AddGroup(traj_angle_group.name, traj_angle_group.dof, '')
# path_config_spec.AddDerivativeGroups(1, False);
# path_config_spec.AddDerivativeGroups(2, False);
# path_config_spec.AddGroup('owd_blend_radius', 1, 'next')
path_config_spec.ResetGroupOffsets()
new_traj = openravepy.RaveCreateTrajectory(manipulator.GetRobot().GetEnv(), '')
new_traj.Init(path_config_spec)
for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
ignore_col_with.Enable(oldstate)
with manipulator.GetRobot():
manipulator.SetActive()
waypoint_ind = 0
for t in numpy.arange(0, traj_duration, delta_t):
traj_sample = traj.Sample(t)
waypoint = traj_config_spec.ExtractJointValues(traj_sample, manipulator.GetRobot(), manipulator.GetArmIndices())
manipulator.SetDOFValues(waypoint)
#if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot()):
for body in manipulator.GetRobot().GetEnv().GetBodies():
if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot(), body):
collided_with_obj = True
break
if collided_with_obj:
break
else:
#waypoint = numpy.append(waypoint,t)
new_traj.Insert(int(waypoint_ind), waypoint, path_config_spec)
waypoint_ind += 1
manipulator.GetRobot().ExecuteTrajectory(new_traj, execute = True, retime=True, blend=True)
return collided_with_obj
# Trajectory is aborted by OWD because we felt a force.
except exceptions.TrajectoryAborted:
return True
def initialize(attach_viewer=False, sim=True, user_id='human', env=None):
"""Initialize the Human Robot"""
prpy.logger.initialize_logging()
if not env:
env = Environment()
#Setup Manipulators
with env:
robot = env.ReadKinBodyXMLFile('robots/man1.dae')
robot.SetName(user_id) #needed in order to have different humans in the same env
env.AddKinBody(robot)
robot.left_arm = robot.GetManipulator('leftarm')
robot.left_arm.hand = robot.left_arm.GetEndEffector()
robot.left_hand = robot.left_arm.hand
robot.right_arm = robot.GetManipulator('rightarm')
robot.right_arm.hand = robot.right_arm.GetEndEffector()
robot.right_hand = robot.right_arm.hand
bind_subclass(robot, Robot, robot_name=user_id)
bind_subclass(robot.left_arm, Manipulator)
bind_subclass(robot.right_arm, Manipulator)
bind_subclass(robot.left_arm.hand, HumanHand, manipulator=robot.left_arm, sim=True)
bind_subclass(robot.right_arm.hand, HumanHand, manipulator=robot.right_arm, sim=True)
#Setup Controller
args = 'IdealController'
affine_dofs=0
delegate_controllerr = RaveCreateController(env, args)
if delegate_controllerr is None:
message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.right_arm.GetName(), args)
raise openrave_exception(message)
robot.multicontroller.AttachController(delegate_controllerr, robot.right_arm.GetArmIndices(), affine_dofs)
delegate_controllerl = RaveCreateController(env, args)
if delegate_controllerl is None:
message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.left_arm.GetName(), args)
raise openrave_exception(message)
robot.multicontroller.AttachController(delegate_controllerl, robot.left_arm.GetArmIndices(), affine_dofs)
#Setup IK
ikmodel_left = InverseKinematicsModel(
robot,
iktype=IkParameterizationType.Transform6D,
manip=robot.left_arm)
if not ikmodel_left.load():
ikmodel_left.autogenerate()
ikmodel_right = InverseKinematicsModel(
robot,
iktype=IkParameterizationType.Transform6D,
manip=robot.right_arm)
if not ikmodel_right.load():
ikmodel_right.autogenerate()
#Setup planning pipeline
robot.planner = Sequence(
SnapPlanner(),
VectorFieldPlanner(),
CBiRRTPlanner())
robot.simplifier = None
robot.retimer = HauserParabolicSmoother() # hack
robot.smoother = HauserParabolicSmoother()
robot.actions = prpy.action.ActionLibrary()
#Setup viewer. Default to load rviz
if env.GetViewer() is None:
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
#Fallback on qtcoin if rviz couldnt load
if env.GetViewer is None:
logger.warning('Loading the Rviz viewer failed. Falling back on qtcoin')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}"'.format(
attach_viewer))
#Remove ROS Logging since loading Rviz might have added it
prpy.logger.remove_ros_logger()
#changing orientation
with env:
robotLocation = numpy.array([[ -1. , 0. , 0. , 0.],
[ 0. , 0. , 1. , 0.],
[ 0. , 1. , 0. , 0.],
[ 0. , 0. , 0. , 1. ]])
robot.SetTransform(robotLocation)
return env, robot
