def jnt_state_cb(self, msg):
velocity = {}
for name, vel in zip(msg.name, msg.velocity):
velocity[name] = vel
# lowpass for measured velocity
self.vel_trans = 0.5 * self.vel_trans + 0.5 * (velocity[
self.r_joint] + velocity[self.l_joint]) * self.wheel_radius / 2.0
self.vel_rot = 0.5 * self.vel_rot + 0.5 * (
velocity[self.r_joint] - velocity[self.l_joint]
) * self.wheel_radius / (2.0 * self.wheel_basis)
# velocity commands
vel_trans = self.vel_trans_desi + self.k_trans * (self.vel_trans_desi -
self.vel_trans)
vel_rot = self.vel_rot_desi + self.k_rot * (self.vel_rot_desi -
self.vel_rot)
# wheel commands
l_cmd = JointCommand()
l_cmd.name = self.l_joint
l_cmd.effort = self.vel_to_eff * (
vel_trans / self.wheel_radius -
vel_rot * self.wheel_basis / self.wheel_radius)
self.pub.publish(l_cmd)
r_cmd = JointCommand()
r_cmd.name = self.r_joint
r_cmd.effort = self.vel_to_eff * (
vel_trans / self.wheel_radius +
vel_rot * self.wheel_basis / self.wheel_radius)
self.pub.publish(r_cmd)
def jnt_state_cb(self, msg):
if self.isActive:
if not self.initialized:
self.vel_rot_desi = 0.0
self.vel_trans_desi = 0.0
self.initialized = True
velocity = {}
for name, vel in zip(msg.name, msg.velocity):
velocity[name] = vel
# lowpass for measured velocity
self.vel_trans = 0.5*self.vel_trans + 0.5*(velocity[self.r_joint] + velocity[self.l_joint])*self.wheel_radius/2.0
self.vel_rot = 0.5*self.vel_rot + 0.5*(velocity[self.r_joint] - velocity[self.l_joint])*self.wheel_radius/(2.0*self.wheel_basis)
# velocity commands
vel_trans = self.vel_trans_desi + self.k_trans*(self.vel_trans_desi - self.vel_trans)
vel_rot = self.vel_rot_desi + self.k_rot*(self.vel_rot_desi - self.vel_rot)
# wheel commands
l_cmd = JointCommand()
l_cmd.name = self.l_joint
l_cmd.effort = self.vel_to_eff*(vel_trans/self.wheel_radius - vel_rot*self.wheel_basis/self.wheel_radius)
self.pub.publish(l_cmd)
r_cmd = JointCommand()
r_cmd.name = self.r_joint
r_cmd.effort = self.vel_to_eff*(vel_trans/self.wheel_radius + vel_rot*self.wheel_basis/self.wheel_radius)
self.pub.publish(r_cmd)
def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.name:
self.vel = 0.5 * self.vel + 0.5 * vel
if not self.initialized:
self.pos_desi = pos
self.initialized = True
cmd = JointCommand()
cmd.name = self.name
cmd.effort = 190.0 * (self.pos_desi - pos) - 4.0 * self.vel
print 'Joint at %f, going to %f, commanding joint %f'%(pos,self.pos_desi, cmd.effort)
self.pub.publish(cmd)
def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.name:
self.vel = 0.5 * self.vel + 0.5 * vel
if not self.initialized:
self.pos_desi = pos
self.initialized = True
cmd = JointCommand()
cmd.name = self.name
cmd.effort = 190.0 * (self.pos_desi - pos) - 4.0 * self.vel
print 'Joint at %f, going to %f, commanding joint %f' % (
pos, self.pos_desi, cmd.effort)
self.pub.publish(cmd)
def execute_cb(self, msg):
for i in range(0, len(msg.joint_names_goal)):
cmd = JointCommand()
cmd.name = msg.joint_names_goal[i]
cmd.effort = self.efforts[msg.joint_names_goal[i]]
rospy.logwarn(str(msg.joint_names_goal[i]))
rospy.logwarn(str(msg.joint_position_angles_goal[i]))
if msg.joint_position_angles_goal[i] < self.joint_positions[
msg.joint_names_goal[i]]:
cmd.effort = cmd.effort * -1
rospy.logwarn('publishe ' + str(cmd.effort) + ' auf ' +
str(cmd.name))
self.arm_joint_pub.publish(cmd)
while True:
if abs(msg.joint_position_angles_goal[i] -
self.joint_positions[msg.joint_names_goal[i]]
) > self.goal_threshold:
rospy.logwarn(
str(
abs(msg.joint_position_angles_goal[i] -
self.joint_positions[msg.joint_names_goal[i]]))
)
continue
else:
cmd.effort = 0.0
self.arm_joint_pub.publish(cmd)
time.sleep(.1)
self.arm_joint_pub.publish(cmd)
break
rospy.logwarn('reached')
self._result.joint_position_angles_result = [
self.joint_positions[MOTOR_A], self.joint_positions[MOTOR_B],
self.joint_positions[GRIPPER]
]
self._sas.set_succeeded(self._result)
rospy.logwarn("ArmPositionServer succeeded")
def jnt_state_cb(self, msg):
for name, pos, vel in zip(msg.name, msg.position, msg.velocity):
if name == self.motor1name:
if not self.motor1initialized:
self.motor1pos_desi = pos
self.motor1initialized = True
self.motor1error = self.motor1pos_desi - pos
self.motor1delta_err = self.motor1prev_err - self.motor1error
self.motor1integral_err += self.motor1error
if self.motor1integral_err > 0.5:
self.motor1integral_err = 0.5
if self.motor1integral_err < -0.5:
self.motor1integral_err = -0.5
self.motor1p_out = self.motor1error * self.motor1KP
self.motor1i_out = self.motor1integral_err * self.motor1KI
self.motor1d_out = self.motor1delta_err * self.motor1KD
self.motor1output = self.motor1p_out + self.motor1i_out + self.motor1d_out
if self.motor1output > 0.65:
self.motor1output = 0.65
if self.motor1output < -0.65:
self.motor1output = -0.65
self.motor1prev_err = self.motor1error
cmd = JointCommand()
cmd.name = self.motor1name
cmd.effort = self.motor1output ;
#print 'Motor Joint 1 at %f, going to %f, commanding joint %f'%(pos,self.motor1pos_desi, cmd.effort)
self.pub.publish(cmd)
if name == self.motor2name:
if not self.motor2initialized:
self.motor2pos_desi = pos
self.motor2initialized = True
self.motor2error = self.motor2pos_desi - pos
self.motor2delta_err = self.motor2prev_err - self.motor2error
self.motor2integral_err += self.motor2error
if self.motor2integral_err > 0.5:
self.motor2integral_err = 0.5
if self.motor2integral_err < -0.5:
self.motor2integral_err = -0.5
self.motor2p_out = self.motor2error * self.motor2KP
self.motor2i_out = self.motor2integral_err * self.motor2KI
self.motor2d_out = self.motor2delta_err * self.motor2KD
self.motor2output = self.motor2p_out + self.motor2i_out + self.motor2d_out
if self.motor2output > 0.65:
self.motor2output = 0.65
if self.motor2output < -0.65:
self.motor2output = -0.65
self.motor2prev_err = self.motor2error
cmd = JointCommand()
cmd.name = self.motor2name
cmd.effort = self.motor2output ;
#print 'Motor Joint 2 at %f, going to %f, commanding joint %f'%(pos,self.motor2pos_desi, cmd.effort)
self.pub.publish(cmd)
if name == self.motor3name:
if not self.motor3initialized:
self.motor3pos_desi = pos
self.motor3initialized = True
self.motor3error = self.motor3pos_desi - pos
self.motor3delta_err = self.motor3prev_err - self.motor3error
self.motor3integral_err += self.motor3error
if self.motor3integral_err > 0.5:
self.motor3integral_err = 0.5
if self.motor3integral_err < -0.5:
self.motor3integral_err = -0.5
self.motor3p_out = self.motor3error * self.motor3KP
self.motor3i_out = self.motor3integral_err * self.motor3KI
self.motor3d_out = self.motor3delta_err * self.motor3KD
self.motor3output = self.motor3p_out + self.motor3i_out + self.motor3d_out
if self.motor3output > 0.65:
self.motor3output = 0.65
if self.motor3output < -0.65:
self.motor3output = -0.65
self.motor3prev_err = self.motor3error
cmd = JointCommand()
cmd.name = self.motor3name
cmd.effort = self.motor3output ;
#print 'Motor Joint 3 at %f, going to %f, commanding joint %f'%(pos,self.motor3pos_desi, cmd.effort)
self.pub.publish(cmd)