def send_moving_base_xform_PID_vel(controller, R, t, v_in): """For a moving base robot model, send a command to move to the rotation matrix R and translation t by setting the velocity Note: with the reflex model, can't currently set hand commands and linear base commands simultaneously """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll, pitch, yaw = so3.rpy(R) q[3] = yaw q[4] = pitch q[5] = roll v = controller.getCommandedVelocity() for i in range(3): v[i] = v_in[i] v[3] = v_in[5] # yaw v[4] = v_in[4] # pitch v[5] = v_in[3] # roll controller.setPIDCommand(q, v)
def dalk_path_generation(path_filename, calib_filename): data = np.loadtxt(path_filename) oct2robot = np.loadtxt(calib_filename)[0, :].reshape((4, 4)) robot2oct = np.linalg.inv(oct2robot) robot_target = [] xyzrpy = [] # dalk_frame = (so3.from_rpy([0, 0, -np.pi/2]), [52.5 * 0.0254, 6.5 * 0.0251, 0.75 * 0.0254]) dalk_frame = (so3.from_rpy([0, np.pi, 0]), [0.0, 0.0, 0.0]) for row in data[::1]: x_target = row[12 + 6:12 + 6 + 6] # xyzrpy.append(x_target) T = np.eye(4) T = rotation_matrix(x_target[3], [1, 0, 0]).dot( rotation_matrix(x_target[4], [0, 1, 0])).dot( rotation_matrix(x_target[5], [0, 0, 1])) T[:3, 3] = x_target[0:3] T = robot2oct.dot(T) T[:3, 3] = T[:3, 3] * 1 T = oct2robot.dot(T) robot_target.append(T) T2rpyxyz = list(se3.from_homogeneous(T)[1]) + list( so3.rpy(se3.from_homogeneous(T)[0])) print(T2rpyxyz) xyzrpy.append(T2rpyxyz) # T_m = se3.mul(se3.inv(dalk_frame), se3.from_homogeneous(T)) # T_h = se3.homogeneous(T_m) # T2rpyxyz = list(T_m[1]) + list(so3.rpy(T_m[0])) # xyzrpy.append(T2rpyxyz) # robot_target.append(T_h) return robot_target, xyzrpy
def test_rpy(self): R = so3.from_quaternion( (-4.32978e-17, -0.707107, 4.32978e-17, 0.707107)) r, p, y = so3.rpy(R) self.assertAlmostEqual(r, 0.0) self.assertAlmostEqual(p, 1.5707963267948966) self.assertAlmostEqual(y, 3.141592653589793)
def set_moving_base_xform(robot, R, t): """For a moving base robot model, set the current base rotation matrix R and translation t. (Note: if you are controlling a robot during simulation, use send_moving_base_xform_command) """ q = robot.getConfig() for i in range(3): q[i] = t[i] roll, pitch, yaw = so3.rpy(R) q[3] = yaw q[4] = pitch q[5] = roll robot.setConfig(q)
def set_moving_base_xform(robot,R,t): """For a moving base robot model, set the current base rotation matrix R and translation t. (Note: if you are controlling a robot during simulation, use send_moving_base_xform_command) """ q = robot.getConfig() for i in range(3): q[i] = t[i] roll,pitch,yaw = so3.rpy(R) q[3]=yaw q[4]=pitch q[5]=roll robot.setConfig(q)
def send_moving_base_xform_PID(controller, R, t): """For a moving base robot model, send a command to move to the rotation matrix R and translation t by setting the PID setpoint Note: with the reflex model, can't currently set hand commands and linear base commands simultaneously """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll, pitch, yaw = so3.rpy(R) q[3] = yaw q[4] = pitch q[5] = roll v = controller.getCommandedVelocity() controller.setPIDCommand(q, [10] * 6 + [0] * (len(q) - 6)) #v)
def send_moving_base_xform_linear(controller, R, t, dt): """For a moving base robot model, send a command to move to the rotation matrix R and translation t using linear interpolation over the duration dt. Note: with the reflex model, can't currently set hand commands and linear base commands simultaneously """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll, pitch, yaw = so3.rpy(R) q[3] = yaw q[4] = pitch q[5] = roll controller.setLinear(q, dt)
def send_moving_base_xform_PID(controller,R,t): """For a moving base robot model, send a command to move to the rotation matrix R and translation t by setting the PID setpoint Note: with the reflex model, can't currently set hand commands and linear base commands simultaneously """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll,pitch,yaw = so3.rpy(R) q[3]=yaw q[4]=pitch q[5]=roll v = controller.getCommandedVelocity() controller.setPIDCommand(q,v)
def send_moving_base_xform_linear(controller,R,t,dt): """For a moving base robot model, send a command to move to the rotation matrix R and translation t using linear interpolation over the duration dt. Note: with the reflex model, can't currently set hand commands and linear base commands simultaneously """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll,pitch,yaw = so3.rpy(R) q[3]=yaw q[4]=pitch q[5]=roll controller.setLinear(q,dt)
def send_moving_base_xform_PID(controller, R, t): """For a moving base robot model, send a command to move to the rotation matrix R and translation t by setting the PID setpoint Note: can't currently set robot commands and linear base commands simultaneously. If you want to do this, you'll have to wrap your own base trajectory controller. """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll, pitch, yaw = so3.rpy(R) q[3] = yaw q[4] = pitch q[5] = roll v = controller.getCommandedVelocity() controller.setPIDCommand(q, v)
def send_moving_base_xform_linear(controller, R, t, dt): """For a moving base robot model, send a command to move to the rotation matrix R and translation t using linear interpolation over the duration dt. Note: can't currently set robot commands and linear base commands simultaneously. If you want to do this, you'll have to wrap your own base trajectory controller. """ q = controller.getCommandedConfig() for i in range(3): q[i] = t[i] roll, pitch, yaw = so3.rpy(R) q[3] = yaw q[4] = pitch q[5] = roll controller.setLinear(q, dt)
def EditTransform(value=None, xmin=None, xmax=None, labels=None, klampt_widget=None, xform_name='edited_xform', axis_length=DEFAULT_AXIS_LENGTH, axis_width=DEFAULT_AXIS_WIDTH, callback=None): """Creates a Jupyter widget for interactive editing of a rigid transform point Args: value (klampt.se3 element), optional: the initial value of the transform (klampt.se3 element). If given as (R,t), the R and t members must be lists and will hold the edited values. xmin/xmax (list of 3 floats, optional): the minimum and maximum of the translation labels (list of strs, optional): if given, the labels of roll,pitch,yaw and x,y,z klampt_widget (KlamptWidget, optional): the KlamptWidget visualization to update, or None if you don't want to visualize the point. xform_name (str, optional): the name of the xform in the visualization world to edit. axis_length,axis_width (float, optional): the length and width of the visualized widget callback (function, optional): a function callback((R,t)) called when a DOF's value has changed. Returns: VBox: a widget that can be displayed as you like """ if value is None: value = se3.identity() else: if not isinstance(value, (tuple, list)): raise ValueError("value must be a 2-element sequence") if len(value) != 2: raise ValueError("value must be a 2-element sequence") if len(value[0]) != 9: raise ValueError("value[0] must be a 9-element list") if len(value[1]) != 3: raise ValueError("value[1] must be a 3-element list") if labels is None: labels = ['roll', 'pitch', 'yaw', 'x', 'y', 'z'] if xmin is None: xmin = [-5, -5, -5] elif isinstance(xmin, (int, float)): xmin = [xmin, xmin, xmin] if xmax is None: xmax = [5, 5, 5] elif isinstance(xmax, (int, float)): xmax = [xmax, xmax, xmax] if len(xmin) != 3: raise ValueError("xmin must be a 3-element list") if len(xmax) != 3: raise ValueError("xmax must be a 3-element list") if klampt_widget: klampt_widget.addXform(name=xform_name, length=axis_length, width=axis_width) klampt_widget.setTransform(name=xform_name, R=value[0], t=value[1]) rpy = list(so3.rpy(value[0])) def _do_rotation_change(index, element): rpy[index] = element value[0][:] = so3.from_rpy(rpy) if klampt_widget: klampt_widget.setTransform(name=xform_name, R=value[0], t=value[1]) if callback: callback(value) def _do_translation_change(index, element): value[1][index] = element if klampt_widget: klampt_widget.setTransform(name=xform_name, R=value[0], t=value[1]) if callback: callback(value) elems = [] for i in range(3): elems.append( widgets.FloatSlider(description=labels[i], value=rpy[i], min=0, max=math.pi * 2, step=0.001)) elems[-1].observe(lambda v, i=i: _do_rotation_change(i, v['new']), 'value') for i in range(3): elems.append( widgets.FloatSlider(description=labels[3 + i], value=value[1][i], min=xmin[i], max=xmax[i], step=0.001)) elems[-1].observe(lambda v, i=i: _do_translation_change(i, v['new']), 'value') return widgets.VBox(elems)
def EditTransform(value=None,xmin=None,xmax=None,labels=None, klampt_widget=None,xform_name='edited_xform',axis_length=DEFAULT_AXIS_LENGTH,axis_width=DEFAULT_AXIS_WIDTH, callback=None): """Creates a Jupyter widget for interactive editing of a rigid transform point Args: value (klampt.se3 element), optional: the initial value of the transform (klampt.se3 element). If given as (R,t), the R and t members must be lists and will hold the edited values. xmin/xmax (list of 3 floats, optional): the minimum and maximum of the translation labels (list of strs, optional): if given, the labels of roll,pitch,yaw and x,y,z klampt_widget (KlamptWidget, optional): the KlamptWidget visualization to update, or None if you don't want to visualize the point. xform_name (str, optional): the name of the xform in the visualization world to edit. axis_length,axis_width (float, optional): the length and width of the visualized widget callback (function, optional): a function callback((R,t)) called when a DOF's value has changed. Returns: VBox: a widget that can be displayed as you like """ if value is None: value = se3.identity() else: if not isinstance(value,(tuple,list)): raise ValueError("value must be a 2-element sequence") if len(value) != 2: raise ValueError("value must be a 2-element sequence") if len(value[0]) != 9: raise ValueError("value[0] must be a 9-element list") if len(value[1]) != 3: raise ValueError("value[1] must be a 3-element list") if labels is None: labels = ['roll','pitch','yaw','x','y','z'] if xmin is None: xmin = [-5,-5,-5] elif isinstance(xmin,(int,float)): xmin = [xmin,xmin,xmin] if xmax is None: xmax = [5,5,5] elif isinstance(xmax,(int,float)): xmax = [xmax,xmax,xmax] if len(xmin) != 3: raise ValueError("xmin must be a 3-element list") if len(xmax) != 3: raise ValueError("xmax must be a 3-element list") if klampt_widget: klampt_widget.addXform(name=xform_name,length=axis_length,width=axis_width) klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1]) rpy = list(so3.rpy(value[0])) def _do_rotation_change(index,element): rpy[index] = element value[0][:] = so3.from_rpy(rpy) if klampt_widget: klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1]) if callback: callback(value) def _do_translation_change(index,element): value[1][index] = element if klampt_widget: klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1]) if callback: callback(value) elems = [] for i in range(3): elems.append(widgets.FloatSlider(description=labels[i],value=rpy[i],min=0,max=math.pi*2,step=0.001)) elems[-1].observe(lambda v,i=i:_do_rotation_change(i,v['new']),'value') for i in range(3): elems.append(widgets.FloatSlider(description=labels[3+i],value=value[1][i],min=xmin[i],max=xmax[i],step=0.001)) elems[-1].observe(lambda v,i=i:_do_translation_change(i,v['new']),'value') return widgets.VBox(elems)
def integrate_velocities(controller, sim, dt): """The make() function returns a 1-argument function that takes a SimRobotController and performs whatever processing is needed when it is invoked.""" global start_time global now_dur global syn_curr global syn_next global entered_once global got_syn if not entered_once: syn_vec = controller.getCommandedConfig() if not syn_vec: got_syn = False entered_once = False else: got_syn = True entered_once = True syn_curr = syn_vec[34] start_time = sim.getTime() else: syn_curr = syn_next now_dur = sim.getTime() - start_time # print 'The current simulation duration is', now_dur rob = sim.controller(0).model() palm_curr = mv_el.get_moving_base_xform(rob) R = palm_curr[0] t = palm_curr[1] if DEBUG: print 'The current palm rotation is ', R print 'The current palm translation is ', t print 'The simulation dt is ', dt # Converting to rpy euler = so3.rpy(R) # Checking if list empty and returning false if not got_syn: return False, None, None if DEBUG: print 'The integration time is ', dt print 'The adaptive velocity of hand is ', global_vars.hand_command print 'The adaptive twist of palm is \n', global_vars.arm_command # print 'The commanded position of the hand encoder (in memory) is ', syn_curr # print 'The present position of the hand encoder (sensed) is ', controller.getCommandedConfig()[34] v = rob.getVelocity() print 'The actual velocity of the robot is ', v # print 'The present pose of the palm is \n', palm_curr # print 'The present position of the palm is ', t, 'and its orientation is', euler # Getting linear and angular velocities lin_vel_vec = global_vars.arm_command.linear ang_vel_vec = global_vars.arm_command.angular lin_vel = [lin_vel_vec.x, lin_vel_vec.y, lin_vel_vec.z] ang_vel = [ang_vel_vec.x, ang_vel_vec.y, ang_vel_vec.z] ang_vel_loc = so3.apply(so3.inv(R), ang_vel) # rotating ang vel to local frame # Transform ang vel into rpy vel euler_vel = transform_ang_vel(euler, ang_vel_loc) palm_vel = [] palm_vel.extend(lin_vel) palm_vel.extend([euler_vel[0], euler_vel[1], euler_vel[2]]) # appending syn_vel = global_vars.hand_command # Integrating syn_next = syn_curr + syn_vel * int_const_syn * dt t_next = vectorops.madd(t, lin_vel, int_const_t * dt) euler_next = vectorops.madd(euler, euler_vel, int_const_eul * dt) # Saturating synergy if syn_next >= 1.0: syn_next = 1.0 # Convert back for send xform palm_R_next = so3.from_rpy(euler_next) palm_t_next = t_next palm_next = (palm_R_next, palm_t_next) if DEBUG or True: print 'euler is ', euler, ' and is of type ', type(euler) print 'euler_vel is ', euler_vel, ' and is of type ', type(euler_vel) print 'euler_next is ', euler_next, ' and is of type ', type( euler_next) # # print 't is ', t, ' and is of type ', type(t) # print 't_next is ', t_next, ' and is of type ', type(t_next) # # print 'R is ', R, ' and is of type ', type(R) # print 'palm_R_next is ', palm_R_next, ' and is of type ', type(palm_R_next) # # print 'palm_curr is ', palm_curr, ' and is of type ', type(palm_curr) # print 'palm_next is ', palm_next, ' and is of type ', type(palm_next) return True, syn_next, palm_next, syn_vel, palm_vel
def test_rpy(self): R = so3.from_quaternion((-4.32978e-17, -0.707107,4.32978e-17,0.707107)) r,p,y = so3.rpy(R) self.assertAlmostEqual(r,0.0) self.assertAlmostEqual(p,1.5707963267948966) self.assertAlmostEqual(y,3.141592653589793)