def swing_and_measure(pose):
#break out pointers...
env=pose.robot.GetEnv()
robot=pose.robot
pose.send()
maxF=300
while not kbhit.kbhit(True):
env.StepSimulation(openhubo.TIMESTEP)
h=[]
for s in robot.GetAttachedSensors():
force=s.GetSensor().GetSensorData().force
torque=s.GetSensor().GetSensorData().torque
if s.GetName().find('Palm')>-1:
if force[2]<1 and force[2]>-1:
CoPx=0
CoPz=0
else:
CoPx=-torque[2]/force[1]
CoPz=-torque[0]/force[1]
localCoP=[CoPx,0,CoPz,1]
#ASSUME that COM of dummy end-effector body is "contact center"
else:
if force[2]<1 and force[2]>-1:
force[2]=sign(force[2])
CoPx=-torque[1]/force[2]
CoPy=-torque[0]/force[2]
#ASSUME that COM of dummy end-effector body is "contact center"
localCoP=[CoPx,CoPy,0,1]
cop=s.GetAttachingLink().GetTransform()*mat(array(localCoP)).T
r=norm(force)/maxF
h.append(env.plot3(cop[:-1].T,.01,[r,r,r/2.],True))
def swing_and_measure(pose):
#break out pointers...
env = pose.robot.GetEnv()
robot = pose.robot
pose.send()
maxF = 300
while not kbhit.kbhit(True):
env.StepSimulation(openhubo.TIMESTEP)
h = []
for s in robot.GetAttachedSensors():
force = s.GetSensor().GetSensorData().force
torque = s.GetSensor().GetSensorData().torque
if s.GetName().find('Palm') > -1:
if force[2] < 1 and force[2] > -1:
CoPx = 0
CoPz = 0
else:
CoPx = -torque[2] / force[1]
CoPz = -torque[0] / force[1]
localCoP = [CoPx, 0, CoPz, 1]
#ASSUME that COM of dummy end-effector body is "contact center"
else:
if force[2] < 1 and force[2] > -1:
force[2] = sign(force[2])
CoPx = -torque[1] / force[2]
CoPy = -torque[0] / force[2]
#ASSUME that COM of dummy end-effector body is "contact center"
localCoP = [CoPx, CoPy, 0, 1]
cop = s.GetAttachingLink().GetTransform() * mat(array(localCoP)).T
r = norm(force) / maxF
h.append(env.plot3(cop[:-1].T, .01, [r, r, r / 2.], True))
def wait_start():
print "Press Enter to abort, starting simulation in ... "
for x in range(5,0,-1):
print '{}...'.format(x)
for k in range(10):
time.sleep(.1)
k=False
if kbhit.kbhit():
k=kbhit.getch()
if k:
return False
print "Starting simulation!"
return True
def wait_start():
print "Press Enter to abort, starting simulation in ... "
for x in range(5, 0, -1):
print '{}...'.format(x)
for k in range(10):
time.sleep(.1)
k = False
if kbhit.kbhit():
k = kbhit.getch()
if k:
return False
print "Starting simulation!"
return True
def preview_traj(self):
#Assume that robot is in initial position now
T0=self.base.GetTransform()
s=self.robot.CreateRobotStateSaver()
for k in xrange(self.srcdata.shape[0]):
pose_map={key:self.srcdata[k,v] for (key,v) in self.joint_map.items()}
pose_array=array([pose_map[dof] if pose_map.has_key(dof) else 0.0 for dof in xrange(self.robot.GetDOF())])
values=pose_array*self.joint_signs+self.joint_offsets
self.robot.SetDOFValues(values)
T=self.get_transform(T0,self.srcdata[k,0:6])
self.robot.SetTransform(T)
_time.sleep(self.timestep/2.)
if kbhit.kbhit():
kbhit.getch()
_oh.pause()
s.Restore()
def preview_traj(self):
#Assume that robot is in initial position now
T0=self.base.GetTransform()
s=self.robot.CreateRobotStateSaver()
for k in xrange(self.srcdata.shape[0]):
pose_map={key:self.srcdata[k,v] for (key,v) in self.joint_map.items()}
pose_array=array([pose_map[dof] if pose_map.has_key(dof) else 0.0 for dof in xrange(self.robot.GetDOF())])
values=pose_array*self.joint_signs+self.joint_offsets
self.robot.SetDOFValues(values)
T=self.get_transform(T0,self.srcdata[k,0:6])
self.robot.SetTransform(T)
_time.sleep(self.timestep/2.)
if kbhit.kbhit():
kbhit.getch()
_oh.pause()
s.Restore()
timestep=0.0005
#Set PD control gains (probably will need a lot of tuning, and these values are not gauranteed to work!)
Kp=10
#Normalize Kd wrt timestep
Kd=Kp*0.1*timestep
error=zeros(robot.GetDOF())
#Collect masses of bodies attached at joint as a very crude scale factor for gains
masses=array([j.GetHierarchyParentLink().GetMass() + j.GetHierarchyChildLink().GetMass() for j in robot.GetJoints()])
#TODO: better way might be to figure out how much weight a joint has to bear? or use something better than PD :-)A
print "Beginning torque control loop, hit a key to stop..."
#TODO: getch-based loop interrupt
while not kbhit.kbhit():
#Calculate error in pose
with env:
pose=robot.GetDOFValues()
#Difference between current error and last value
derror=((desiredpose-pose)-error)/timestep
error=desiredpose-pose
cmd=(Kp*error+Kd*derror)*masses
"""Finally, set all DOF to have their new torque value. The "True"
argument is important (it specifies that torques should be added
rather than overwritten!)."""
robot.SetDOFTorques(cmd,True)
env.StepSimulation(timestep)
Kp = 10
#Normalize Kd wrt timestep
Kd = Kp * 0.1 * timestep
error = zeros(robot.GetDOF())
#Collect masses of bodies attached at joint as a very crude scale factor for gains
masses = array([
j.GetHierarchyParentLink().GetMass() + j.GetHierarchyChildLink().GetMass()
for j in robot.GetJoints()
])
#TODO: better way might be to figure out how much weight a joint has to bear? or use something better than PD :-)A
print "Beginning torque control loop, hit a key to stop..."
#TODO: getch-based loop interrupt
while not kbhit.kbhit():
#Calculate error in pose
with env:
pose = robot.GetDOFValues()
#Difference between current error and last value
derror = ((desiredpose - pose) - error) / timestep
error = desiredpose - pose
cmd = (Kp * error + Kd * derror) * masses
"""Finally, set all DOF to have their new torque value. The "True"
argument is important (it specifies that torques should be added
rather than overwritten!)."""
robot.SetDOFTorques(cmd, True)
env.StepSimulation(timestep)
from optparse import OptionParser
import time
import openravepy
import openhubo
from openhubo import kbhit
if __name__ == "__main__":
"""Modified version of contact display from openrave examples"""
(env,options)=openhubo.setup('qtcoin',True)
env.SetDebugLevel(2)
time.sleep(.25)
[robot,ctrl,ind,ref,recorder]=openhubo.load_scene(env,options)
stop=False
# Set the floor and other bodies to be slightly transparent to better visualize interpenetrations
for b in env.GetBodies():
if not b == robot:
openhubo.set_robot_color(b,trans=.3)
while not stop:
handles=openhubo.plot_contacts(robot)
#handles=openhubo.plot_dirs(robot)
openhubo.sleep(.05)
if kbhit.kbhit(True):
stop=True
__license__ = 'Apache License, Version 2.0'
from optparse import OptionParser
import time
import openravepy
import openhubo
from openhubo import kbhit
if __name__ == "__main__":
"""Modified version of contact display from openrave examples"""
(env, options) = openhubo.setup('qtcoin', True)
env.SetDebugLevel(2)
time.sleep(.25)
[robot, ctrl, ind, ref, recorder] = openhubo.load_scene(env, options)
stop = False
# Set the floor and other bodies to be slightly transparent to better visualize interpenetrations
for b in env.GetBodies():
if not b == robot:
openhubo.set_robot_color(b, trans=.3)
while not stop:
handles = openhubo.plot_contacts(robot)
#handles=openhubo.plot_dirs(robot)
openhubo.sleep(.05)
if kbhit.kbhit(True):
stop = True
