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