def main():
env, robot = fetchpy.initialize(None, None, 'qtcoin')
x = robot.arm.GetDOFValues()
joint_target = np.array([0., 1., 3.14159265, 2., 0., -2., 0.])
z = robot.arm.GetArmIndices()
#robot.SetDOFValues(y, z)
planner = or_trajopt.TrajoptPlanner()
robot.SetActiveDOFs(z)
traj = planner.PlanToConfiguration(robot, joint_target)
waypoints = traj.GetAllWaypoints2D()
for waypoint in waypoints:
robot.arm.SetDOFValues(np.array(waypoint))
rospy.sleep(1.)
def main(initJointState, rate):
# Initialize fetchpy
env, robot = fetchpy.initialize(None, None, 'qtcoin')
# Set the arm as the active portion of the robot
activeDOFs = robot.arm.GetArmIndices()
robot.SetActiveDOFs(activeDOFs)
# initialize fetchpy robot joint state
robot.arm.SetDOFValues(initJointState)
# Set joint target
joint_target = np.array([0., 0., 0, 0., 0., 0., 0.])
# Set the TrajOpt planner
planner = or_trajopt.TrajoptPlanner()
# Plan motion
traj = planner.PlanToConfiguration(robot, joint_target)
waypoints = traj.GetAllWaypoints2D()
for waypoint in waypoints:
robot.arm.SetDOFValues(np.array(waypoint))
rate.sleep()
def __init__(self):
self.client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.client.wait_for_server()
self.jointNames = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
self.trajectory = JointTrajectory()
self.trajectory.joint_names = self.jointNames[:]
self.goal = FollowJointTrajectoryGoal()
self.timeStep = 0.5
self.timeVector = None
self.env, self.robot = fetchpy.initialize(None, None, 'qtcoin')
self.activeDOFs = self.robot.arm.GetArmIndices()
self.robot.SetActiveDOFs(self.activeDOFs)
self.planner = Sequence(TrajoptPlanner(), OMPLPlanner('RRTConnect'))
self.traj = None
self.waypoints = None
self.waypointVel = None
self.waypointAcc = None
#!/usr/bin/env python
from openravepy import *
import fetchpy
import rospy
import numpy as np
from fetchwbp import util, patterns
from fetchwbp.planner import MJWBPlanner
from fetchwbp.plotting import plottingPoints
#from MJwbp.planner import MJWBPlanner
if __name__ == '__main__':
rospy.init_node('fetchpy')
fetch_args = {'sim': True, 'viewer': 'qtcoin'}
env, robot = fetchpy.initialize(**fetch_args)
viewer = env.GetViewer()
originaxes = misc.DrawAxes(env, [1, 0, 0, 0, 0, 0, 0], dist=1, linewidth=2)
keep_going = True
#Moving the arm to a proper pose to start trajectory
to_Table = ([
0., 0.70503065, -0.81321057, 0.44084394, 1.52903305, -0.37976212,
0.92392059, 0.8291418
])
robot.arm_torso.PlanToConfiguration(to_Table, execute=True)
raw_input("Press enter to Create Pattern")
#Creating pattern and plot the points we need our robot to go through, starting from the current
#pose of the gripper
poses = patterns.createZigZagPattern(
#!/usr/bin/env python
PKG = 'fetchpy'
import roslib
roslib.load_manifest(PKG)
import numpy, unittest
import fetchpy
env, robot = fetchpy.initialize(sim=True)
class ArmTest(unittest.TestCase):
def setUp(self):
self._env, self._robot = env, robot
self._arm = robot.arm
self._indices = self._arm.GetArmIndices()
self._num_dofs = len(self._indices)
def test_SetStiffness_DoesNotThrow(self):
self._wam.SetStiffness(0.0)
self._wam.SetStiffness(0.5)
self._wam.SetStiffness(1.0)
def test_SetStiffness_InvalidStiffnessThrows(self):
self.assertRaises(Exception, self._arm.SetStiffness, (-0.2))
self.assertRaises(Exception, self._arm.SetStiffness, (1.2))
def test_Servo_DoesNotThrow(self):
self._arm.Servo(0.1 * numpy.ones(self._num_dofs))
def test_Servo_IncorrectSizeThrows(self):
velocity_small = 0.1 * numpy.ones(self._num_dofs - 1)
#!/usr/bin/env python
import fetchpy
import numpy
import rospy
if __name__ == '__main__':
rospy.init_node('fetchpy')
env, robot = fetchpy.initialize(sim=False)
keep_going = True
while keep_going:
print "\n\nWelcome to Pose Control:\n \
Press 0 to dock the arm.\n \
Press 1 to dock the arm and the torso.\n \
Press 2 to say something.\n \
Press 3 to straight the arm.\n \
Press 4 to go to cleaing wall.\n \
Press 5 to wave.\n \
Press 6 to do I love you.\n \
Press 7 to say Yes.\n \
Press 8 to say No.\n \
Press 9 to quit."
user_input = int(raw_input("Gesture? "))
if user_input == 0:
print 'Going to dock the arm!\n'
robot.arm.PlanToNamedConfiguration('arm_dock', execute=True)
robot.gripper.CloseHand()