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()))
#!/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()
