def leaveHandle():
print "Wyjazd z klamki..."
Goal = PyKDL.Vector(
doorR_x + 0.1 * math.cos(alfa) + 0.25 * math.sin(alfa),
doorR_y + 0.1 * math.sin(alfa) - 0.25 * math.cos(alfa),
doorR_z + 0.1)
T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0.25 * math.pi + alfa),
Goal)
if not velma.moveCartImpRight(
[T_B_Door], [10.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7),
PyKDL.Vector(0.7, 0.7, 0.7)),
start_time=0.5,
path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1),
PyKDL.Vector(1, 1, 1))):
exitError(8)
if velma.waitForEffectorRight() != 0:
print "Velma: :p"
rospy.sleep(0.5)
resetPoseTol()
rospy.sleep(0.5)
def twist_cb(self, msg):
twist = PyKDL.Twist()
state_button_1 = msg.paddles[1].buttons[0]
state_button_4 = msg.paddles[1].buttons[4]
state_button_5 = msg.paddles[1].buttons[5]
if state_button_1:
para_vel = -1
else:
para_vel = 1
if state_button_5:
para_ang = 1
else:
para_ang = 0
if state_button_4:
para_trans = 1
else:
para_trans = 0
twist[0] = msg.paddles[1].joy[0]
twist[1] = msg.paddles[1].joy[1]
twist[2] = para_vel*msg.paddles[1].trigger
twist[3] = 0#para_ang*msg.paddles[1].joy[0]#msg.angular.x
twist[4] = 0#para_ang*msg.paddles[1].joy[1]#msg.angular.y
twist[5] = 0#para_ang*para_vel*msg.paddles[1].trigger#msg.angular.z
self.twist = self._T_baseoffset_inverse * twist
pass
def moveToDoor():
print "Ruch do dzwiczek..."
Goal = PyKDL.Vector(
1 / 3 * doorL_x + 2 / 3 * doorR_x + 0.5 * math.cos(alfa),
1 / 3 * doorL_y + 2 / 3 * doorR_y + 0.5 * math.sin(alfa),
doorR_z + 0.1)
T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(alfa), Goal)
if not velma.moveCartImpRight(
[T_B_Door], [10.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7),
PyKDL.Vector(0.7, 0.7, 0.7)),
start_time=0.5,
path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1),
PyKDL.Vector(1, 1, 1))):
exitError(8)
if velma.waitForEffectorRight() != 0:
print "Velma: Dotykam drzwi :p"
rospy.sleep(0.5)
resetPoseTol()
rospy.sleep(0.5)
def moveToHandle():
print "Ruch do klamki..."
DoorR = velma.getTf(
"B", "right_door"
) #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame
doorR_x, doorR_y, doorR_z = DoorR.p
DoorL = velma.getTf(
"B", "left_door"
) #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame
doorL_x, doorL_y, doorL_z = DoorL.p
position = velma.getLastJointState()
Goal = PyKDL.Vector(doorL_x, doorL_y, doorR_z + 0.1)
T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal)
if not velma.moveCartImpRight(
[T_B_Door], [10.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7),
PyKDL.Vector(0.7, 0.7, 0.7)),
start_time=0.5,
path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1),
PyKDL.Vector(1, 1, 1))):
exitError(8)
if velma.waitForEffectorRight() != 0:
print "Velma: Dotykam klamki :p"
rospy.sleep(0.5)
resetPoseTol()
rospy.sleep(0.5)
def forceAdmittanceControl(self, forceCtrlDir):
# compute the desired force magnitude
curTime = (2 * np.pi) / self.forceProfile['period'] * time()
sinamplitudeMag = np.sin(curTime) * abs(
self.forceProfile['amplitude']) / 2 + abs(
self.forceProfile['amplitude']) / 2
cosVal = np.cos(curTime)
if cosVal <= 0 and self.lastCosVal >= 0:
self.maxSinePublisher.publish(Bool(True))
self.lastCosVal = cosVal
fRefMag = sinamplitudeMag + abs(self.forceProfile['fBiasMag'])
# compute desired force
desiredForce = forceCtrlDir * fRefMag
# get the current force
forceError = self.fCurrent + desiredForce
# apply admittance gain to compute motion
desiredTwist = PyKDL.Twist()
desiredTwist.vel = PyKDL.Vector(\
forceError.x()*self.forceProfile['admittanceGains'][0],
forceError.y()*self.forceProfile['admittanceGains'][1],
forceError.z()*self.forceProfile['admittanceGains'][2])
velNorm = desiredTwist.vel.Norm()
if velNorm > self.resolvedRatesConfig['absVelMax']:
desiredTwist.vel.Normalize()
desiredTwist.vel = desiredTwist.vel \
* self.resolvedRatesConfig['absVelMax']
desiredTwist.rot = PyKDL.Vector(0.0, 0.0, 0.0)
return desiredTwist
def impedStearing(T_B_Trd,imp_list,pt): # cart
global move_time
print("Rozpoczecie sterowania impendacyjnego---------------------------")
# This test/tutorial executes simple impedance commands in Cartesian impedance mode.
# To see the tool frame add 'tf' in rviz and enable 'right_arm_tool' frame.
# At every state switch to cart_imp, the tool frames are reset.
# Also, the tool impedance parameters are reset to 1500N/m in every
# direction for linear stiffness and to 150Nm/rad in every direction for angular
# stiffness, i.e. (1500,1500,1500,150,150,150)."
"""TEST WALNIECIA W SZAFKE"""
t=countTime(T_B_Trd)
actual_gripper_position = velma.getTf("B", "Tr") #aktualna pozycja chwytaka
print("Moving right wrist to pose defined in world frame...")
"""Tak tutaj dalem 3 zamiast move_time*t bo dla malych ruchow wywalalo errora bo za maly czas wyliczalo"""
if not velma.moveCartImpRight([T_B_Trd], [3], None, None, imp_list, [0.5], max_wrench=makeWrench([5,5,5], [5,5,5]),
start_time=0.01, path_tol=PyKDL.Twist(PyKDL.Vector(pt,pt,pt), PyKDL.Vector(pt,pt,pt))):
exitError(13)
if velma.waitForEffectorRight() != 0: #zglaszane jak chwytak nie moze osiagnac zadanej pozycji
return actual_gripper_position.p[0] , actual_gripper_position.p[1], actual_gripper_position.p[2]
print("Pose reached, there was no collision")
return actual_gripper_position.p[0] , actual_gripper_position.p[1], actual_gripper_position.p[2]
def load_manipulator_comd_config(self,config_data):
# this func load configurations about the manipulator command that the user_interface gives
# device frame offsets
rot_X = ast.literal_eval(config_data.get(
'user_interface', 'TF_device2view_rot_X'))
rot_Y = ast.literal_eval(config_data.get(
'user_interface', 'TF_device2view_rot_Y'))
rot_Z = ast.literal_eval(config_data.get(
'user_interface', 'TF_device2view_rot_Z'))
pos = ast.literal_eval(config_data.get(
'user_interface', 'TF_device2view_trans'))
frame = PyKDL.Frame.Identity()
frame.M = PyKDL.Rotation(
rot_X[0],rot_Y[0],rot_Z[0],
rot_X[1],rot_Y[1],rot_Z[1],
rot_X[2],rot_Y[2],rot_Z[2])
frame.P = PyKDL.Vector(pos[0],pos[1],pos[2])
self.tf_dev2view = frame
# command_type
self.command_type = config_data.get(
'user_interface', 'command_type')
# subscriber - manipulator command from user input
rostopic_manipulator_comd = config_data.get(
'user_interface', 'rostopic_manipulator_comd')
if (self.command_type=="pose"):
self.manipulator_comd = []
rospy.Subscriber(
rostopic_manipulator_comd, Pose, self.manipulator_comd_pose_CB)
else:
self.manipulator_comd = PyKDL.Twist()
rospy.Subscriber(
rostopic_manipulator_comd, Twist, self.manipulator_comd_twist_CB)
def inverse(self, t, TGB):
if len(t) < self._num_jnts:
rospy.logerr_throttle(
5,
"[ServoIk.inverse] Wrong number of joints published. Published: {} Existing: {}"
.format(len(t), self._num_jnts))
return None
else:
joints_array = kdl.JntArray(self._num_jnts)
for i in xrange(self._num_jnts):
joints_array[i] = t[i]
joint_velocities_array = kdl.JntArray(self._num_jnts)
FEB = kdl.Frame()
FGB = kdl.Frame(
kdl.Rotation.Quaternion(TGB[1][0], TGB[1][1], TGB[1][2],
TGB[1][3]),
kdl.Vector(TGB[0][0], TGB[0][1], TGB[0][2]))
self._fk_p.JntToCart(joints_array, FEB)
FEGB = FGB * FEB.Inverse()
iterations = 0
while (FEGB.p.Norm() > LINEAR_TOLERANCE or FEGB.M.GetRotAngle()[0] > ANGULAR_TOLERANCE) \
and iterations < MAX_ITERATIONS:
iterations += 1
twist = kdl.Twist(FEGB.p, FEGB.M.GetRot())
self._ik_v.CartToJnt(joints_array, twist,
joint_velocities_array)
maximum = 0.0
for i in xrange(self._num_jnts):
if joint_velocities_array[i] > maximum:
maximum = joint_velocities_array[i]
if maximum > SATURATION:
reduction = SATURATION / maximum
else:
reduction = 1.0
for i in xrange(self._num_jnts):
joints_array[i] += reduction * joint_velocities_array[i]
self._fk_p.JntToCart(joints_array, FEB)
FEGB = FGB * FEB.Inverse()
logdebug('amount of iterations needed: %s', iterations)
if iterations >= MAX_ITERATIONS:
return None
solution = []
for i in xrange(self._num_jnts):
while joints_array[i] < pi:
joints_array[i] += 2.0 * pi
while joints_array[i] > pi:
joints_array[i] -= 2.0 * pi
if joints_array[i] < self.lower_limits[i] \
or joints_array[i] > self.upper_limits[i]:
return None
solution.append(joints_array[i])
return solution
def impedMove(T_B_Trd, imp_list, pt, tm=3): # cart
print "Rozpoczecie sterowania impendacyjnego---------------------------"
# actual_gripper_position = velma.getTf("B", "Tr") # aktualna pozycja chwytaka
print "Moving right wrist to pose defined in world frame..."
my_wrench = makeWrench([5, 5, 5], [5, 5, 5])
my_twist = PyKDL.Twist(PyKDL.Vector(pt, pt, pt),
PyKDL.Vector(pt, pt, pt))
rospy.sleep(0.5)
print(len(imp_list))
"""Tak tutaj dalem 3 zamiast move_time*t bo dla malych ruchow wywalalo errora bo za maly czas wyliczalo"""
if not velma.moveCartImpRight([T_B_Trd], [tm],
None,
None,
imp_list, [0.5],
max_wrench=my_wrench,
start_time=0.01,
path_tol=my_twist):
exitError(13)
if velma.waitForEffectorRight(
) != 0: # zglaszane jak chwytak nie moze osiagnac zadanej pozycji
print("I think it worked?")
return #actual_gripper_position.p[0], actual_gripper_position.p[1], actual_gripper_position.p[2]
print "Pose reached, there was no collision"
return #actual_gripper_position.p[0], actual_gripper_position.p[1], actual_gripper_position.p[2]
def to_kdl_twist(self):
vel = PyKDL.Vector(self._velocity['linear'][0],
self._velocity['linear'][1],
self._velocity['linear'][2])
rot = PyKDL.Vector(self._velocity['angular'][0],
self._velocity['angular'][1],
self._velocity['angular'][2])
return PyKDL.Twist(vel, rot)
def twist_to_kdl(twist):
t = PyKDL.Twist()
t.vel[0] = twist.linear.x
t.vel[1] = twist.linear.y
t.vel[2] = twist.linear.z
t.rot[0] = twist.angular.x
t.rot[1] = twist.angular.y
t.rot[2] = twist.angular.z
return t
def __init__(self):
self.goal_traj = []
self.real_traj = []
self.acc_limit = PyKDL.Twist()
self.acc_limit.vel[0] = 0.01
self.acc_limit.vel[1] = 0.01
self.acc_limit.rot[2] = 0.01
self.done = True
urdf = rospy.get_param('robot_description')
self.urdf = URDF.from_xml_string(hacky_urdf_parser_fix(urdf))
limits = PyKDL.Twist()
limits.vel[0] = self.urdf.joint_map[x_joint].limit.velocity
limits.vel[1] = self.urdf.joint_map[y_joint].limit.velocity
limits.rot[2] = self.urdf.joint_map[z_joint].limit.velocity
self._min_vel = -limits
self._max_vel = limits
self.cmd = None
self.last_cmd = PyKDL.Twist()
self.pose_history = None
self.cmd_vel_sub = rospy.Publisher('~cmd_vel', Twist, queue_size=10)
self.debug_vel = rospy.Publisher('debug_vel', Twist, queue_size=10)
js = rospy.wait_for_message('/joint_states', JointState)
self.x_index_js = js.name.index(x_joint)
self.y_index_js = js.name.index(y_joint)
self.z_index_js = js.name.index(z_joint)
self.current_goal = None
self.js_sub = rospy.Subscriber('/joint_states',
JointState,
self.js_cb,
queue_size=10)
self.state_pub = rospy.Publisher('{}/state'.format(name_space),
JointTrajectoryControllerState,
queue_size=10)
self.server = SimpleActionServer(
'{}/follow_joint_trajectory'.format(name_space),
FollowJointTrajectoryAction,
self.execute_cb,
auto_start=False)
self.server.start()
def twist_cb(self, msg):
twist = PyKDL.Twist()
twist[0] = msg.linear.x
twist[1] = msg.linear.y
twist[2] = msg.linear.z
twist[3] = msg.angular.x
twist[4] = msg.angular.y
twist[5] = msg.angular.z
self.twist = self._T_baseoffset_inverse * twist
pass
def __init__(self,
object_name,
goal,
goal_twist=kdl.Twist(),
p_gain=1.0,
d_gain=1.0):
self._object_name = object_name
self._goal_pose = goal
self._goal_twist = goal_twist
self._p_gain = p_gain
self._d_gain = d_gain
def test_compute_twist(self):
for _ in range(100):
T0 = br.transform.random()
vel = np.random.rand(3)
rot = np.random.rand(3)
kdl_twist = PyKDL.Twist(PyKDL.Vector(*vel), PyKDL.Vector(*rot))
F0 = posemath.fromMatrix(T0)
F1 = PyKDL.addDelta(F0, kdl_twist)
T1 = posemath.toMatrix(F1)
twist = ru.transforms.compute_twist(F0, F1)
np.testing.assert_allclose(twist, np.hstack((vel, rot)))
def __init__(self, name):
pose_topic_name = name + 'pose'
twist_topic_name = name + 'twist'
button_topic_name = name + 'button'
force_topic_name = name + 'force_feedback'
self._active = False
self._scale = 0.0002
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.twist = PyKDL.Twist()
# This offset is to align the pitch with the view frame
R_off = Rotation.RPY(0.0, 0, 0)
self._T_baseoffset = Frame(R_off, Vector(0, 0, 0))
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.clutch_button_pressed = False # Used as Position Engage Clutch
self.gripper_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_topic_name,
PoseStamped,
self.pose_cb,
queue_size=1)
self._twist_sub = rospy.Subscriber(twist_topic_name,
Twist,
self.twist_cb,
queue_size=1)
###### Commented
self._button_sub = rospy.Subscriber(button_topic_name,
DeviceButtonEvent,
self.buttons_cb,
queue_size=1)
self._force_pub = rospy.Publisher(force_topic_name,
DeviceFeedback,
queue_size=1)
self.switch_psm = False
self._button_msg_time = rospy.Time.now()
self._switch_psm_duration = rospy.Duration(0.5)
print('Creating Geomagic Device Named: ', name, ' From ROS Topics')
self._msg_counter = 0
def test_transform(self):
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1.0
t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0)
t.header.stamp = builtin_interfaces.msg.Time(sec=2)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
out = b.lookup_transform('a', 'b', rclpy.time.Time(seconds=2),
rclpy.time.Duration(seconds=2))
self.assertEqual(out.transform.translation.x, 1)
self.assertEqual(out.transform.rotation.x, 1)
self.assertEqual(out.header.frame_id, 'a')
self.assertEqual(out.child_frame_id, 'b')
v = PyKDL.Vector(1, 2, 3)
out = b.transform(
tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.x(), 0)
self.assertEqual(out.y(), -2)
self.assertEqual(out.z(), -3)
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
out = b.transform(
tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.p.x(), 0)
self.assertEqual(out.p.y(), -2)
self.assertEqual(out.p.z(), -3)
# TODO(tfoote) check values of rotation
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(
tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.vel.x(), 1)
self.assertEqual(out.vel.y(), -8)
self.assertEqual(out.vel.z(), 2)
self.assertEqual(out.rot.x(), 4)
self.assertEqual(out.rot.y(), -5)
self.assertEqual(out.rot.z(), -6)
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(
tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.force.x(), 1)
self.assertEqual(out.force.y(), -2)
self.assertEqual(out.force.z(), -3)
self.assertEqual(out.torque.x(), 4)
self.assertEqual(out.torque.y(), -8)
self.assertEqual(out.torque.z(), -4)
def resolvedRates(self, currentPose, desiredPose):
# compute pose error (result in kdl.twist format)
poseError = PyKDL.diff(currentPose, desiredPose)
posErrNorm = poseError.vel.Norm()
rotErrNorm = poseError.rot.Norm()
# compute velocity magnitude based on position error norm
if posErrNorm > self.resolvedRatesConfig['tolPos']:
tolPosition = self.resolvedRatesConfig['tolPos']
lambdaVel = self.resolvedRatesConfig['velRatio']
if posErrNorm > (lambdaVel * tolPosition):
velMag = self.resolvedRatesConfig['velMax']
else:
velMax = self.resolvedRatesConfig['velMax']
velMin = self.resolvedRatesConfig['velMin']
velMag = (velMin \
+ (posErrNorm - tolPosition) \
* (velMax-velMin) / (tolPosition * (lambdaVel-1)))
else:
velMag = 0.0
# compute angular velocity based on rotation error norm
if rotErrNorm > self.resolvedRatesConfig['tolRot']:
tolRotation = self.resolvedRatesConfig['tolRot']
lambdaRot = self.resolvedRatesConfig['rotRatio']
if rotErrNorm > (lambdaRot * tolRotation):
angVelMag = self.resolvedRatesConfig['angVelMax']
else:
angVelMax = self.resolvedRatesConfig['angVelMax']
angVelMin = self.resolvedRatesConfig['angVelMin']
angVelMag = (angVelMin \
+ (rotErrNorm - tolRotation) \
* (angVelMax - angVelMin) \
/ (tolRotation * (lambdaRot-1)))
else:
angVelMag = 0.0
# The resolved rates is implemented as Nabil Simaan's notes
# apply both the velocity and angular velocity in the error pose direction
desiredTwist = PyKDL.Twist()
poseError.vel.Normalize() # normalize to have the velocity direction
desiredTwist.vel = poseError.vel * velMag
poseError.rot.Normalize() # normalize to have the ang vel direction
desiredTwist.rot = poseError.rot * angVelMag
# Check whether we have reached our goal
# goalReached = desiredTwist.vel.Norm() <= self.resolvedRatesConfig['velMin'] \
# and desiredTwist.rot.Norm() <= self.resolvedRatesConfig['angVelMin']
# Ignore rotation for calculating goalReached!
goalReached = desiredTwist.vel.Norm(
) <= self.resolvedRatesConfig['velMin']
return desiredTwist, goalReached
def test_transform(self):
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1
t.transform.rotation.x = 1
t.header.stamp = rospy.Time(2.0)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
out = b.lookup_transform('a', 'b', rospy.Time(2.0),
rospy.Duration(2.0))
self.assertEqual(out.transform.translation.x, 1)
self.assertEqual(out.transform.rotation.x, 1)
self.assertEqual(out.header.frame_id, 'a')
self.assertEqual(out.child_frame_id, 'b')
v = PyKDL.Vector(1, 2, 3)
out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b')
self.assertEqual(out.x(), 0)
self.assertEqual(out.y(), -2)
self.assertEqual(out.z(), -3)
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b')
print(out)
self.assertEqual(out.p.x(), 0)
self.assertEqual(out.p.y(), -2)
self.assertEqual(out.p.z(), -3)
# TODO(tfoote) check values of rotation
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b')
self.assertEqual(out.vel.x(), 1)
self.assertEqual(out.vel.y(), -8)
self.assertEqual(out.vel.z(), 2)
self.assertEqual(out.rot.x(), 4)
self.assertEqual(out.rot.y(), -5)
self.assertEqual(out.rot.z(), -6)
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
self.assertEqual(out.force.x(), 1)
self.assertEqual(out.force.y(), -2)
self.assertEqual(out.force.z(), -3)
self.assertEqual(out.torque.x(), 4)
self.assertEqual(out.torque.y(), -8)
self.assertEqual(out.torque.z(), -4)
def resolvedRates(config, currentPose, desiredPose):
# compute pose error (result in kdl.twist format)
poseError = PyKDL.diff(currentPose, desiredPose)
posErrNorm = poseError.vel.Norm()
rotErrNorm = poseError.rot.Norm()
angVelMag = config['angVelMax']
if rotErrNorm < config['tolRot']:
angVelMag = 0.0
elif rotErrNorm < angVelMag:
angVelMag = rotErrNorm
# compute velocity magnitude based on position error norm
if posErrNorm > config['tolPos']:
tolPosition = config['tolPos']
lambdaVel = config['velRatio']
if posErrNorm > (lambdaVel * tolPosition):
velMag = config['velMax']
else:
velMax = config['velMax']
velMin = config['velMin']
velMag = velMin + (posErrNorm - tolPosition) * \
(velMax - velMin)/(tolPosition*(lambdaVel-1))
else:
velMag = 0.0
# compute angular velocity based on rotation error norm
if rotErrNorm > config['tolRot']:
tolRotation = config['tolRot']
lambdaRot = config['rotRatio']
if rotErrNorm > (lambdaRot * tolRotation):
angVelMag = config['angVelMax']
else:
angVelMax = config['angVelMax']
angVelMin = config['angVelMin']
angVelMag = angVelMin + (rotErrNorm - tolRotation) * \
(angVelMax - angVelMin)/(tolRotation*(lambdaRot-1))
else:
angVelMag = 0.0
# The resolved rates is implemented as Nabil Simaan's notes
# apply both the velocity and angular velocity in the error pose direction
desiredTwist = PyKDL.Twist()
poseError.vel.Normalize() # normalize to have the velocity direction
desiredTwist.vel = poseError.vel * velMag
poseError.rot.Normalize() # normalize to have the ang vel direction
desiredTwist.rot = poseError.rot * angVelMag
return desiredTwist
def moveInCartImpMode(velma, T_B_dest, tol=10):
if not velma.moveCartImpRight(
[T_B_dest], [5.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)),
start_time=0.1,
path_tol=PyKDL.Twist(PyKDL.Vector(tol, tol, tol),
PyKDL.Vector(tol, tol, tol))):
exitError(8)
if velma.waitForEffectorRight() != 0:
if not velma.moveCartImpRightCurrentPos(start_time=0.01):
exitError(9)
return False
else:
return True
def get_qdot_from_twist(self, q, twist):
"""Returns the joint velocities for a given twist
Arguments:
q -- The current robot configuration
twist -- The desired twist
"""
q_arr = kdl.JntArray(self._num_of_joints)
qdot_arr = kdl.JntArray(self._num_of_joints)
kdl_twist = kdl.Twist()
for joint_num in range(self._num_of_joints):
q_arr[joint_num] = q[joint_num]
for joint_num in range(6):
kdl_twist[joint_num] = twist[joint_num]
self._tv_solver.CartToJnt(q_arr, kdl_twist, qdot_arr)
ret = np.array([0.0] * self._num_of_joints)
for joint_num in range(self._num_of_joints):
ret[joint_num] = qdot_arr[joint_num]
return ret
def _command_callback(self, msg):
self._command = np.zeros(6)
rot = self._trans.M
twist = PyKDL.Twist(
PyKDL.Vector(msg.twist.linear.x, msg.twist.linear.y,
msg.twist.linear.z),
PyKDL.Vector(msg.twist.angular.x, msg.twist.angular.y,
msg.twist.angular.z))
twist = self._trans.M * twist
self._command[0] = twist.vel.x()
self._command[1] = twist.vel.y()
self._command[2] = twist.vel.z()
if not self._position_only:
self._command[3] = twist.rot.x()
self._command[4] = twist.rot.y()
self._command[5] = twist.rot.z()
def resolvedRates(self):
# get current and desired robot pose (desired is the top of queue)
currentPose = self.robot.get_current_position()
desiredPose = self.trajectory[0]
# compute pose error (result in kdl.twist format)
poseError = PyKDL.diff(currentPose, desiredPose)
posErrNorm = poseError.vel.Norm()
rotErrNorm = poseError.rot.Norm()
# compute velocity magnitude based on position error norm
if posErrNorm > self.resolvedRatesConfig['tolPos']:
tolPosition = self.resolvedRatesConfig['tolPos']
lambdaVel = self.resolvedRatesConfig['velRatio']
if posErrNorm > (lambdaVel * tolPosition):
velMag = self.resolvedRatesConfig['velMax']
else:
velMax = self.resolvedRatesConfig['velMax']
velMin = self.resolvedRatesConfig['velMin']
velMag = velMin + \
(velMax - velMin)/(tolPosition*(lambdaVel-1))
else:
velMag = 0.0
# compute angular velocity based on rotation error norm
if rotErrNorm > self.resolvedRatesConfig['tolRot']:
tolRotation = self.resolvedRatesConfig['tolRot']
lambdaRot = self.resolvedRatesConfig['rotRatio']
if posErrNorm > (lambdaRot * tolRotation):
angVelMag = self.resolvedRatesConfig['angVelMax']
else:
angVelMax = self.resolvedRatesConfig['angVelMax']
angVelMin = self.resolvedRatesConfig['angVelMin']
angVelMag = angVelMin + \
(angVelMax - angVelMin)/(tolRotation*(lambdaRot-1))
else:
angVelMag = 0.0
# The resolved rates is implemented as Nabil Simaan's notes
# apply both the velocity and angular velocity in the error pose direction
sys_dt = self.resolvedRatesConfig['dt']
desiredTwist = PyKDL.Twist()
poseError.vel.Normalize() # normalize to have the velocity direction
desiredTwist.vel = poseError.vel * velMag * sys_dt
poseError.rot.Normalize() # normalize to have the ang vel direction
desiredTwist.rot = poseError.rot * angVelMag * sys_dt
return desiredTwist
def _check_traj(self, dT):
"""Check if the end-effector has reached the desired position of target waypoint.
"""
_ee_position = self._get_ee_position()
_targ_wp_position = tr.translation_from_matrix(self.traj_list[self.cur_wp_idx])
if self.cur_wp_idx < self.num_wp - 1: # indicate next waypoint
self.cur_wp_idx += 1
self.traj_elapse += dT
elif self.cur_wp_idx == self.num_wp - 1: # robot has reached the last waypoint
# rospy.logwarn('Reached target object')
self.cur_wp_idx = 0
self.isPathPlanned = False
self.traj_elapse = 0
print ('Reset integral twist ...')
self.integ_twist = PyKDL.Twist(PyKDL.Vector(0, 0, 0), PyKDL.Vector(0, 0, 0)) # initialize integral twist
if np.linalg.norm(np.array(self.ee_position) - self._get_target_pose()) <= self.dist_threshold:
rospy.logwarn('Reached target object')
rospy.delete_param('vel_calc')
def move_frame(velma, frame, time_wrist=6, frame_tool=None, time_tool=None):
print "Moving right wrist to pose defined in world frame..."
# if not velma.moveCartImpRight([frame], [3.0], frame_tool_vector, time_tool_vector, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5):
twist = PyKDL.Twist(PyKDL.Vector(0.1, 0.1, 0.1),
PyKDL.Vector(0.1, 0.1, 0.1))
wr = PyKDL.Wrench(PyKDL.Vector(80, 80, 500), PyKDL.Vector(120, 120, 120))
if not velma.moveCartImpRight([frame], [time_wrist],
frame_tool,
time_tool, [wr], [time_wrist],
PyKDL.Wrench(PyKDL.Vector(5, 5, 5),
PyKDL.Vector(5, 5, 5)),
start_time=0.5,
path_tol=twist):
print "wait for three"
exitError(13)
try:
if velma.waitForEffectorRight() != 0:
print "wait for effector right"
return True
except:
rospy.sleep(0.5)
def main():
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1
t.transform.rotation.x = 1
t.header.stamp = rospy.Time(2.0)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
print b.lookup_transform('a', 'b', rospy.Time(2.0), rospy.Duration(2.0))
v = PyKDL.Vector(1, 2, 3)
print b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b')
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
print b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b')
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
print b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b')
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
print b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
def forceAdmittanceControl(self, forceCtrlDir):
# compute the desired force magnitude
sinamplitudeMag = np.sin((2 * np.pi)/self.forceProfile['period'] * time() ) \
* abs(self.forceProfile['amplitude'])+ abs(self.forceProfile['amplitude'])/2
fRefMag = sinamplitudeMag + abs(self.forceProfile['fBiasMag'])
# compute desired force
desiredForce = forceCtrlDir * fRefMag
# get the current force
forceError = self.fCurrent + desiredForce
# apply admittance gain to compute motion
desiredTwist = PyKDL.Twist()
desiredTwist.vel = PyKDL.Vector(\
forceError.x()*self.forceProfile['admittanceGains'][0],
forceError.y()*self.forceProfile['admittanceGains'][1],
forceError.z()*self.forceProfile['admittanceGains'][2])
velNorm = desiredTwist.vel.Norm()
if velNorm > self.resolvedRatesConfig['velMax']:
desiredTwist.vel.Normalize()
desiredTwist.vel = desiredTwist.vel \
* self.resolvedRatesConfig['velMax']
desiredTwist.rot = PyKDL.Vector(0.0, 0.0, 0.0)
return desiredTwist
def leaveCabinet():
print "Oddalenie od szafki..."
Goal = PyKDL.Vector(1 / 3 * doorL_x + 2 / 3 * doorR_x - 0.4,
1 / 3 * doorL_y + 2 / 3 * doorR_y - 0.2,
doorR_z + 0.1 - 0.25)
T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal)
if not velma.moveCartImpRight(
[T_B_Door], [5.0],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7),
PyKDL.Vector(0.7, 0.7, 0.7)),
start_time=0.5,
path_tol=PyKDL.Twist(PyKDL.Vector(1, 1, 1),
PyKDL.Vector(1, 1, 1))):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
rospy.sleep(0.5)
def compute_pose_velocity_cartesian_eular(joint, joint_vel):
# computes the pose and velocity (twist) based on cartesian and euler
# the default eular sequence used is ZYX
# compute the pose (position and orientation)
pos = PyKDL.Vector(joint[0], joint[1], joint[2])
rot = PyKDL.Rotation.EulerZYX(joint[3], joint[4], joint[5])
pose = PyKDL.Frame(rot, pos)
# compute the twist (velocity)
lin_vel = PyKDL.Vector(joint_vel[0], joint_vel[1], joint_vel[2])
rotation_local = PyKDL.Rotation.Identity()
ang_vel = PyKDL.Vector()
for i in range(3, 6):
if (i == 3):
det_rot = joint_vel[i] * rotation_local.UnitZ()
rotation_local = rotation_local * PyKDL.Rotation.RotZ(joint[i])
elif (i == 4):
det_rot = joint_vel[i] * rotation_local.UnitY()
rotation_local = rotation_local * PyKDL.Rotation.RotY(joint[i])
elif (i == 5):
det_rot = joint_vel[i] * rotation_local.UnitX()
ang_vel = ang_vel + det_rot
twist = PyKDL.Twist(lin_vel, ang_vel)
return pose, twist
