def __init__(self):        
     self.brett = PR2()        
     self.sub = rospy.Subscriber("base_traj_controller/command", tm.JointTrajectory, self.callback)
     self.q = Queue()
     self.F = None
     self.stop_requested = False
     self.ctrl_loop_running = False
import sensor_msgs.msg as sm
import rospy
import brett2
from brett2.PR2 import PR2
import os.path as osp
import numpy as np

rospy.init_node("calibrate_gripper_closed")


pr2 = PR2()
pr2.rgrip.set_angle(-1)
pr2.lgrip.set_angle(-1)
pr2.join_all()
rospy.sleep(1)
data_dir = osp.join(osp.dirname(brett2.__file__),"data")
with open(osp.join(data_dir,"l_closed_val"),"w") as fh:
    fh.write(str(pr2.lgrip.get_angle()))
with open(osp.join(data_dir,"r_closed_val"),"w") as fh:
    fh.write(str(pr2.rgrip.get_angle()))
Esempio n. 3
0
#!/usr/bin/env python

import roslib

roslib.load_manifest('rospy')
import rospy

import utils.conversions as conv
from brett2.PR2 import PR2
from brett2.PR2 import mirror_arm_joints
import math
import numpy

if __name__ == '__main__':
    rospy.init_node('ankush')
    bot = PR2()

    bot.lgrip.open()
    bot.rgrip.open()
    fold_reset = [
        math.pi / 32, math.pi / 12, 0, -math.pi / 2, math.pi, -math.pi / 6,
        -math.pi / 4
    ]
    bot.larm.goto_joint_positions(fold_reset)
    bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))

    rospy.spin()