def __init__(self):
# Connect to the gripper using default settings
self.GPR = gp.Gripper()
# self.CS = cs.NP_05B()
self._pos_file = os.path.join(this_dir, "POS",
"chwp_control_positions.txt")
self._read_pos()
self._log = lg.Logging()
return
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("move_group_python_interface", anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("arm")
self.grip = gripper.Gripper()
self.apple_waypoints = [
[
0.5451367000274355, -0.32821038441372863, 1.0488899044786597,
0.10350893462440268, 0.508320223335784, -0.4935066927564904,
0.6981028543168485
],
[
0.5451657086099234, -0.3282929399965255, 1.0487723503326665,
-0.3278006345616776, 0.5943178428525429, 0.4399232595898125,
0.588048102896299
],
[
0.5452244320737287, -0.3281540033308168, 1.0488378293283063,
-0.15065049751417578, 0.7083013461479153, 0.07693755433273919,
0.6853424278250101
],
[
0.573221221500238, -0.2763317234110172, 1.0055928678962514,
-0.1429475505143203, 0.6214671177525085, 0.09204004472284064,
0.764770063170469
],
[
0.666686069471845, -0.2510994994329945, 0.9464406390562232,
-0.15413001688860256, 0.7183905128940186, 0.07212293141185468,
0.6745052198777627
],
[
0.7050196390663038, -0.23763321212800737, 0.9433473170064602,
-0.15388370528942827, 0.7178481509430941, 0.07380329909920871,
0.6749569693514171
],
[
0.7046144274633646, -0.23866339097610856, 0.9400137356466315,
-0.12870444426853989, 0.7234191911280399, 0.09599582145890147,
0.6714794428689606
],
[
0.09442401119644636, -0.39920937033740733, 1.0998431184026416,
0.4708824426631291, 0.49292906441861584, -0.5577945391318891,
0.473451069021537
]
]
from pypose.driver import Driver from pypose.ax12 import P_MOVING, P_GOAL_SPEED_L import arm import gripper import time driver = Driver(port='/dev/tty.usbserial-AR0JW21B') # Import AX-12 register constants # Get "moving" register for servo with ID 1 #is_moving = driver.getReg(1, P_MOVING, 1) # Set the "moving speed" register for servo with ID 1 #speed = 50 # A number between 0 and 1023 #driver.setReg(1, P_GOAL_SPEED_L, [speed%256, speed>>8]) #driver.setReg(2,P_GOAL_SPEED_L, [speed%256, speed>>8]) g = gripper.Gripper() g.move(512) g.close()
def __init__(self, base, objpath, nobj=5):
self.base = base
self.nobj = nobj
self.smiley = loader.loadModel(objpath)
self.smileyCount = 0
self.setupODE()
self.addGround()
self.partlist = []
self.gripper = gripper.Gripper(base=self.base,
odespace=self.odespace,
odeworld=self.odeworld)
# test
# self.pggen = pg.PandaGeomGen()
# self.handlen = 50.0
#
# # palm, no geom
# self.palmbody = OdeBody(self.odeworld)
# palmmass = OdeMass()
# palmmass.setSphere(.2,1)
# self.palmbody.setMass(palmmass)
# self.palmgeom = self.pggen.genBox(10.0, 10.0, 2.0)
# self.palmgeom.reparentTo(self.base.render)
# # linear joint between palm and environment
# self.palmenvjnt = OdeSliderJoint(self.odeworld)
# self.palmenvjnt.attachBody(self.palmbody, 0)
# self.palmenvjnt.setAxis(0,0,-1)
# self.palmenvjnt.setParamVel(0)
# self.palmenvjnt.setParamFMax(1000000)
#
# # fgr0
# self.fgr0body = OdeBody(self.odeworld)
# fgr0mass = OdeMass()
# fgr0mass.setSphere(.1,1)
# self.fgr0body.setMass(palmmass)
# self.fgr0geom = self.pggen.genBox(10.0, 2.0, self.handlen)
# self.fgr0geom.reparentTo(self.base.render)
# fgr0trimesh = OdeTriMeshData(self.fgr0geom, True)
# fgr0geom = OdeTriMeshGeom(self.odespace, fgr0trimesh)
# fgr0geom.setBody(self.fgr0body)
# # linear joint between fgr0 and palm
# self.fgr0palmjnt = OdeSliderJoint(self.odeworld)
# self.fgr0palmjnt.attach(self.fgr0body, self.palmbody)
# self.fgr0palmjnt.setAxis(0,1,0)
# self.fgr0palmjnt.setParamVel(0)
# self.fgr0palmjnt.setParamFMax(1000000)
#
# # fgr1
# self.fgr1body = OdeBody(self.odeworld)
# fgr1mass = OdeMass()
# fgr1mass.setSphere(.1,1)
# self.fgr1body.setMass(palmmass)
# self.fgr1geom = self.pggen.genBox(10.0, 2.0, self.handlen)
# self.fgr1geom.reparentTo(self.base.render)
# fgr1trimesh = OdeTriMeshData(self.fgr1geom, True)
# fgr1geom = OdeTriMeshGeom(self.odespace, fgr1trimesh)
# fgr1geom.setBody(self.fgr1body)
# # linear joint between fgr1 and palm
# self.fgr1palmjnt = OdeSliderJoint(self.odeworld)
# self.fgr1palmjnt.attach(self.fgr1body, self.palmbody)
# self.fgr1palmjnt.setAxis(0,-1,0)
# self.fgr1palmjnt.setParamVel(0)
# self.fgr1palmjnt.setParamFMax(1000000)
#
# self.bopenhand = False
# self.bclosehand = False
#
# self.palmbody.setPosition(0,0,20.0)
# self.fgr0body.setPosition(0,0,20.0)
# self.fgr1body.setPosition(0,0,20.0)
#
# # palm
# pospalm = self.palmbody.getPosition()
# rotpalm = self.palmbody.getRotation()
# matpalm = Mat4(rotpalm)
# matpalm.setRow(3, pospalm)
# self.palmgeom.setMat(matpalm)
# # fgr0
# posfgr0 = self.fgr0body.getPosition()
# rotfgr0= self.fgr0body.getRotation()
# matfgr0 = Mat4(rotfgr0)
# matfgr0.setRow(3, posfgr0)
# self.fgr0geom.setMat(matfgr0)
# # fgr1
# posfgr1 = self.fgr1body.getPosition()
# rotfgr1= self.fgr1body.getRotation()
# matfgr1 = Mat4(rotfgr1)
# matfgr1.setRow(3, posfgr1)
# self.fgr1geom.setMat(matfgr1)
self.simcontrolstart = time.time()
taskMgr.doMethodLater(.1,
self.addSmiley,
"AddSmiley",
extraArgs=[nobj],
appendTask=True)
taskMgr.add(self.updateODE, "UpdateODE")