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