-
Notifications
You must be signed in to change notification settings - Fork 102
Description
I am using a modified WidowXL arm, which I successfully configured in the MoveIt Setup Assistant. I can execute every pose I created without any issues. However, I am facing a problem when trying to perform a static pick and place operation using inverse kinematics.
The pose we are trying to achieve is a valid pose that can be executed through MoveIt without any problems. However, when attempting to reach the same pose using inverse kinematics, it fails. The motion planning either does not generate a valid trajectory or does not execute correctly.
I am using a modified version of the WidowXL arm, and I can provide URDF modifications or any other relevant configurations if needed. Since the pose works fine in MoveIt, are there any specific parameters or settings that might be affecting the IK solver?
System details:
OS: ubuntu 18.04.6
ROS Version: melodic
My code is this :
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander.conversions import pose_to_list
def all_close(goal, actual, tolerance=0.01):
if isinstance(goal, list):
return all(abs(a - g) < tolerance for a, g in zip(actual, goal))
return False
def compute_inverse_kinematics():
rospy.init_node("inverse_kinematics_solver", anonymous=True)
moveit_commander.roscpp_initialize([])
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "arm_manipulator" # Adjust based on your SRDF MoveIt group
move_group = moveit_commander.MoveGroupCommander(group_name)
move_group.set_path_constraints(None) # Remove constraints
move_group.set_planning_time(10.0) # Increase planning time
move_group.set_num_planning_attempts(10) # Allow multiple attempts
move_group.set_max_velocity_scaling_factor(1.0)
move_group.set_max_acceleration_scaling_factor(1.0)
move_group.set_goal_tolerance(0.01)
move_group.allow_replanning(True)
print "Robot group set to:", move_group.get_name()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.1 # sleep position
target_pose.position.y = 0.0
target_pose.position.z = 0.34
target_pose.orientation.x = 0.0
target_pose.orientation.y = -0.366
target_pose.orientation.z = 0.0
target_pose.orientation.w = 0.93
move_group.set_pose_target(target_pose)
move_group.set_start_state_to_current_state() # Ensure valid start state
plan = move_group.plan()
if plan and plan.joint_trajectory.points: # Check if the trajectory contains points
move_group.execute(plan, wait=True) # Explicit execution
print " Plan executed successfully!"
else:
print " MoveIt failed to generate a valid plan"
move_group.stop()
move_group.clear_pose_targets()
joint_values = move_group.get_current_joint_values()
print "Computed Joint Values:", joint_values
print "Planning Frame:", move_group.get_planning_frame()
print "End Effector Link:", move_group.get_end_effector_link()
print "Current End-Effector Pose:", move_group.get_current_pose().pose
print "Available Planners:", move_group.get_interface_description()
print "Robot Model Loaded:", move_group.get_name()
print "Joint Names:", move_group.get_active_joints()
moveit_commander.roscpp_shutdown()
return joint_values
if __name__ == "__main__":
try:
joint_angles = compute_inverse_kinematics()
print "Resulting Joint Angles:", joint_angles
except rospy.ROSInterruptException:
pass
except KeyboardInterrupt:
pass
The warning i get is this : [ WARN] [1741292075.512546270]: Fail: ABORTED: Catastrophic failure
and the error i get from moveit is this : [ERROR] [1741291121.714569183]: Unable to construct goal representation
Any guidance or suggestions would be greatly appreciated. Thanks in advance!
Robot Model
modified widowxl arm
Operating System
Ubuntu 18.04
ROS Distro
ROS 1 Melodic
Steps To Reproduce
No response
Relevant log output
Additional Info
No response