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)
